From c51475640e279067019dc9fe6c81d7473b2039bd Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 14 Oct 2019 15:47:22 -0400 Subject: [PATCH] mavlink: only send ATTITUDE/ATTITUDE_QUATERNION msgs on vehicle_attitude update - fixes #13182 --- src/modules/mavlink/mavlink_messages.cpp | 26 +++++++++--------------- 1 file changed, 10 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index ffcb07291dc5..61a55b8b2288 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1185,7 +1185,6 @@ class MavlinkStreamAttitude : public MavlinkStream MavlinkOrbSubscription *_att_sub; MavlinkOrbSubscription *_angular_velocity_sub; uint64_t _att_time{0}; - uint64_t _angular_velocity_time{0}; /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &) = delete; @@ -1200,18 +1199,16 @@ class MavlinkStreamAttitude : public MavlinkStream bool send(const hrt_abstime t) { - bool updated = false; + vehicle_attitude_s att; - vehicle_attitude_s att{}; - vehicle_angular_velocity_s angular_velocity{}; - updated |= _att_sub->update(&_att_time, &att); - updated |= _angular_velocity_sub->update(&_angular_velocity_time, &angular_velocity); + if (_att_sub->update(&_att_time, &att)) { + vehicle_angular_velocity_s angular_velocity{}; + _angular_velocity_sub->update(&angular_velocity); - if (updated) { mavlink_attitude_t msg{}; const matrix::Eulerf euler = matrix::Quatf(att.q); - msg.time_boot_ms = math::max(angular_velocity.timestamp, att.timestamp) / 1000; + msg.time_boot_ms = att.timestamp / 1000; msg.roll = euler.phi(); msg.pitch = euler.theta(); msg.yaw = euler.psi(); @@ -1267,7 +1264,6 @@ class MavlinkStreamAttitudeQuaternion : public MavlinkStream MavlinkOrbSubscription *_att_sub; MavlinkOrbSubscription *_angular_velocity_sub; uint64_t _att_time{0}; - uint64_t _angular_velocity_time{0}; /* do not allow top copying this class */ MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &) = delete; @@ -1281,17 +1277,15 @@ class MavlinkStreamAttitudeQuaternion : public MavlinkStream bool send(const hrt_abstime t) { - bool updated = false; + vehicle_attitude_s att; - vehicle_attitude_s att{}; - vehicle_angular_velocity_s angular_velocity{}; - updated |= _att_sub->update(&_att_time, &att); - updated |= _angular_velocity_sub->update(&_angular_velocity_time, &angular_velocity); + if (_att_sub->update(&_att_time, &att)) { + vehicle_angular_velocity_s angular_velocity{}; + _angular_velocity_sub->update(&angular_velocity); - if (updated) { mavlink_attitude_quaternion_t msg{}; - msg.time_boot_ms = math::max(angular_velocity.timestamp, att.timestamp) / 1000; + msg.time_boot_ms = att.timestamp / 1000; msg.q1 = att.q[0]; msg.q2 = att.q[1]; msg.q3 = att.q[2];