From bbd051b2468dfc9bb9e48e65e0993e1c39a9a115 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 27 Mar 2019 16:19:13 +0900 Subject: [PATCH] GCS_MAVLink: send OPTICAL_FLOW messages to optical flow driver --- libraries/GCS_MAVLink/GCS.h | 2 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 16 ++++++++++++++++ 2 files changed, 18 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index f0c3354062..ce9ee8d0b0 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -427,6 +427,8 @@ protected: MAV_RESULT handle_command_get_home_position(const mavlink_command_long_t &packet); MAV_RESULT handle_command_do_fence_enable(const mavlink_command_long_t &packet); + void handle_optical_flow(const mavlink_message_t* msg); + // vehicle-overridable message send function virtual bool try_send_message(enum ap_message id); virtual void send_global_position_int(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index b76961ec8f..a1c5017e71 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include "GCS.h" @@ -3014,6 +3015,17 @@ void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t *msg) RC_Channels::set_override(15, packet.chan16_raw, tnow); } +// allow override of RC channel values for HIL or for complete GCS +// control of switch position and RC PWM values. +void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t *msg) +{ + OpticalFlow *optflow = AP::opticalflow(); + if (optflow == nullptr) { + return; + } + optflow->handle_msg(msg); +} + /* handle messages which don't require vehicle specific data */ @@ -3179,6 +3191,10 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg) case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: handle_rc_channels_override(msg); break; + + case MAVLINK_MSG_ID_OPTICAL_FLOW: + handle_optical_flow(msg); + break; } }