Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: pass vision-position-estimate reset_counter to AP_VisualOdom handler

Browse Source
This commit is contained in:
Randy Mackay 2020-04-10 13:38:59 +09:00 committed by Andrew Tridgell
parent 3557104787
commit c39fef6c56
2 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -740,6 +740,7 @@ private:
const float roll,
const float pitch,
const float yaw,
const uint8_t reset_counter,
const uint16_t payload_size);
void lock_channel(const mavlink_channel_t chan, bool lock);

13
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -2943,7 +2943,7 @@ void GCS_MAVLINK::handle_vision_position_estimate(const mavlink_message_t &msg)
mavlink_vision_position_estimate_t m;
mavlink_msg_vision_position_estimate_decode(&msg, &m);
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw,
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.reset_counter,
PAYLOAD_SIZE(chan, VISION_POSITION_ESTIMATE));
}
@ -2952,7 +2952,7 @@ void GCS_MAVLINK::handle_global_vision_position_estimate(const mavlink_message_t
mavlink_global_vision_position_estimate_t m;
mavlink_msg_global_vision_position_estimate_decode(&msg, &m);
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw,
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.reset_counter,
PAYLOAD_SIZE(chan, GLOBAL_VISION_POSITION_ESTIMATE));
}
@ -2961,7 +2961,8 @@ void GCS_MAVLINK::handle_vicon_position_estimate(const mavlink_message_t &msg)
mavlink_vicon_position_estimate_t m;
mavlink_msg_vicon_position_estimate_decode(&msg, &m);
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw,
// vicon position estimate does not include reset counter
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, 0,
PAYLOAD_SIZE(chan, VICON_POSITION_ESTIMATE));
}
@ -2975,6 +2976,7 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use
const float roll,
const float pitch,
const float yaw,
const uint8_t reset_counter,
const uint16_t payload_size)
{
#if HAL_VISUALODOM_ENABLED
@ -2985,7 +2987,7 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use
if (visual_odom == nullptr) {
return;
}
visual_odom->handle_vision_position_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw);
visual_odom->handle_vision_position_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw, reset_counter);
#endif
}
@ -3002,7 +3004,8 @@ void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg)
if (visual_odom == nullptr) {
return;
}
visual_odom->handle_vision_position_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, m.q);
// note: att_pos_mocap does not include reset counter
visual_odom->handle_vision_position_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, m.q, 0);
#endif
}