Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -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_battery2();
#if AP_AHRS_NAVEKF_AVAILABLE
void send_opticalflow(const OpticalFlow &optflow);
void send_opticalflow();
#endif
virtual void send_attitude() 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
*/
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
if (!optflow.healthy()) {
if (optflow == nullptr ||
!optflow->healthy()) {
return;
}
// get rates from sensor
const Vector2f &flowRate = optflow.flowRate();
const Vector2f &bodyRate = optflow.bodyRate();
const Vector2f &flowRate = optflow->flowRate();
const Vector2f &bodyRate = optflow->bodyRate();
const AP_AHRS &ahrs = AP::ahrs();
float hagl = 0;
@ -1509,7 +1512,7 @@ void GCS_MAVLINK::send_opticalflow(const OpticalFlow &optflow)
flowRate.y,
bodyRate.x,
bodyRate.y,
optflow.quality(),
optflow->quality(),
hagl, // ground distance (in meters) set to zero
flowRate.x,
flowRate.y);
@ -3203,6 +3206,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
send_mount_status();
break;
case MSG_OPTICAL_FLOW:
CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);
send_opticalflow();
break;
case MSG_POSITION_TARGET_GLOBAL_INT:
CHECK_PAYLOAD_SIZE(POSITION_TARGET_GLOBAL_INT);
send_position_target_global_int();