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

GCS_MAVLINK: move sending of optical flow status up to base class

Browse Source
This commit is contained in:
Peter Barker 2018-09-02 16:32:50 +10:00 committed by Andrew Tridgell
parent ebf038eb57
commit 38fdcb02a4
2 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -183,7 +183,7 @@ public:
void send_ahrs(); void send_ahrs();
void send_battery2(); void send_battery2();
#if AP_AHRS_NAVEKF_AVAILABLE #if AP_AHRS_NAVEKF_AVAILABLE
void send_opticalflow(const OpticalFlow &optflow); void send_opticalflow();
#endif #endif
virtual void send_attitude() const; virtual void send_attitude() const;
void send_autopilot_version() const; void send_autopilot_version() const;

18
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1481,16 +1481,19 @@ MAV_RESULT GCS_MAVLINK::_set_mode_common(const MAV_MODE _base_mode, const uint32
/* /*
send OPTICAL_FLOW message send OPTICAL_FLOW message
*/ */
void GCS_MAVLINK::send_opticalflow(const OpticalFlow &optflow) void GCS_MAVLINK::send_opticalflow()
{ {
const OpticalFlow *optflow = AP::opticalflow();
// exit immediately if no optical flow sensor or not healthy // exit immediately if no optical flow sensor or not healthy
if (!optflow.healthy()) { if (optflow == nullptr ||
!optflow->healthy()) {
return; return;
} }
// get rates from sensor // get rates from sensor
const Vector2f &flowRate = optflow.flowRate(); const Vector2f &flowRate = optflow->flowRate();
const Vector2f &bodyRate = optflow.bodyRate(); const Vector2f &bodyRate = optflow->bodyRate();
const AP_AHRS &ahrs = AP::ahrs(); const AP_AHRS &ahrs = AP::ahrs();
float hagl = 0; float hagl = 0;
@ -1509,7 +1512,7 @@ void GCS_MAVLINK::send_opticalflow(const OpticalFlow &optflow)
flowRate.y, flowRate.y,
bodyRate.x, bodyRate.x,
bodyRate.y, bodyRate.y,
optflow.quality(), optflow->quality(),
hagl, // ground distance (in meters) set to zero hagl, // ground distance (in meters) set to zero
flowRate.x, flowRate.x,
flowRate.y); flowRate.y);
@ -3203,6 +3206,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
send_mount_status(); send_mount_status();
break; break;
case MSG_OPTICAL_FLOW:
CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);
send_opticalflow();
break;
case MSG_POSITION_TARGET_GLOBAL_INT: case MSG_POSITION_TARGET_GLOBAL_INT:
CHECK_PAYLOAD_SIZE(POSITION_TARGET_GLOBAL_INT); CHECK_PAYLOAD_SIZE(POSITION_TARGET_GLOBAL_INT);
send_position_target_global_int(); send_position_target_global_int();