fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 22:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: only send ATTITUDE/ATTITUDE_QUATERNION msgs on vehicle_attitude update

Browse Source 
- fixes #13182
This commit is contained in:
Daniel Agar
2019-10-14 15:47:22 -04:00
committed by GitHub
parent fb12ddb69a
commit c51475640e
1 changed files with 10 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+10 -16
View File
@@ -1185,7 +1185,6 @@ private:
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 @@ protected:
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 @@ private:
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 @@ protected:
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];