Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: support VISION_SPEED_ESTIMATE

This commit is contained in:
chobits 2020-05-13 16:29:55 +08:00 committed by Randy Mackay
parent dee095b4a4
commit ff6e4c4f9a
2 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -773,6 +773,7 @@ private:
const float yaw,
const uint8_t reset_counter,
const uint16_t payload_size);
void handle_vision_speed_estimate(const mavlink_message_t &msg);
void lock_channel(const mavlink_channel_t chan, bool lock);

19
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -2992,6 +2992,21 @@ void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg)
#endif
}
void GCS_MAVLINK::handle_vision_speed_estimate(const mavlink_message_t &msg)
{
#if HAL_VISUALODOM_ENABLED
AP_VisualOdom *visual_odom = AP::visualodom();
if (visual_odom == nullptr) {
return;
}
mavlink_vision_speed_estimate_t m;
mavlink_msg_vision_speed_estimate_decode(&msg, &m);
const Vector3f vel = {m.x, m.y, m.z};
uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.usec, PAYLOAD_SIZE(chan, VISION_SPEED_ESTIMATE));
visual_odom->handle_vision_speed_estimate(m.usec, timestamp_ms, vel, m.reset_counter);
#endif
}
void GCS_MAVLINK::handle_command_ack(const mavlink_message_t &msg)
{
AP_AccelCal *accelcal = AP::ins().get_acal();
@ -3247,6 +3262,10 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
handle_att_pos_mocap(msg);
break;
case MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE:
handle_vision_speed_estimate(msg);
break;
case MAVLINK_MSG_ID_SYSTEM_TIME:
handle_system_time_message(msg);
break;