From fd3037bbb7af0e09c4c95ea6dee13d4406be100f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 25 Mar 2018 21:45:48 +1100 Subject: [PATCH] GCS_MAVLink: add support for alternative protocol this allows an alternative to MAVLink to be installed on a GCS port. This is used for BLHeli passthru support --- libraries/GCS_MAVLink/GCS.cpp | 20 ++++++++++++++++++++ libraries/GCS_MAVLink/GCS.h | 19 ++++++++++++++++++- libraries/GCS_MAVLink/GCS_Common.cpp | 27 ++++++++++++++++++++++++++- libraries/GCS_MAVLink/GCS_MAVLink.cpp | 5 +++++ libraries/GCS_MAVLink/GCS_MAVLink.h | 1 + 5 files changed, 70 insertions(+), 2 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 05f01bae9f..905e993556 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -39,4 +39,24 @@ void GCS::send_ekf_origin(const Location &ekf_origin) const FOR_EACH_ACTIVE_CHANNEL(send_ekf_origin(ekf_origin)); } +/* + install an alternative protocol handler. This allows another + protocol to take over the link if MAVLink goes idle. It is used to + allow for the AP_BLHeli pass-thru protocols to run on hal.uartA + */ +bool GCS::install_alternative_protocol(mavlink_channel_t c, GCS_MAVLINK::protocol_handler_fn_t handler) +{ + if (c >= num_gcs()) { + return false; + } + if (chan(c).alternative.handler) { + // already have one installed - we may need to add support for + // multiple alternative handlers + return false; + } + chan(c).alternative.handler = handler; + return true; +} + + #undef FOR_EACH_ACTIVE_CHANNEL diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index bf85ba65f8..11cbac7e53 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -92,6 +92,7 @@ enum ap_message { class GCS_MAVLINK { public: + friend class GCS; GCS_MAVLINK(); void update(uint32_t max_time_us=1000); void init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan); @@ -218,6 +219,9 @@ public: // return current packet overhead for a channel static uint8_t packet_overhead_chan(mavlink_channel_t chan); + // alternative protocol function handler + FUNCTOR_TYPEDEF(protocol_handler_fn_t, bool, uint8_t, AP_HAL::UARTDriver *); + protected: // overridable method to check for packet acceptance. Allows for @@ -327,6 +331,7 @@ protected: bool try_send_gps_message(enum ap_message id); void send_hwstatus(); void handle_data_packet(mavlink_message_t *msg); + private: float adjust_rate_for_stream_trigger(enum streams stream_num); @@ -467,6 +472,15 @@ private: void load_signing_key(void); bool signing_enabled(void) const; static void save_signing_timestamp(bool force_save_now); + + // alternative protocol handler support + struct { + GCS_MAVLINK::protocol_handler_fn_t handler; + uint32_t last_mavlink_ms; + uint32_t last_alternate_ms; + bool active; + } alternative; + }; /// @class GCS @@ -536,6 +550,10 @@ public: // static frsky_telem pointer to support queueing text messages AP_Frsky_Telem *frsky_telemetry_p; + + // install an alternative protocol handler + bool install_alternative_protocol(mavlink_channel_t chan, GCS_MAVLINK::protocol_handler_fn_t handler); + private: static GCS *_singleton; @@ -555,7 +573,6 @@ private: // true if we are running short on time in our main loop bool _out_of_time; - }; GCS &gcs(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 3f08db60a8..eb2c23c61d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -861,6 +861,7 @@ GCS_MAVLINK::update(uint32_t max_time_us) mavlink_message_t msg; mavlink_status_t status; uint32_t tstart_us = AP_HAL::micros(); + uint32_t now_ms = AP_HAL::millis(); hal.util->perf_begin(_perf_update); @@ -871,15 +872,39 @@ GCS_MAVLINK::update(uint32_t max_time_us) for (uint16_t i=0; i protocol_timeout) { + /* + we have an alternative protocol handler installed and we + haven't parsed a MAVLink packet for 4 seconds. Try + parsing using alternative handler + */ + if (alternative.handler(c, mavlink_comm_port[chan])) { + alternative.last_alternate_ms = now_ms; + gcs_alternative_active[chan] = true; + } + + /* + we may also try parsing as MAVLink if we haven't had a + successful parse on the alternative protocol for 4s + */ + if (now_ms - alternative.last_alternate_ms <= protocol_timeout) { + continue; + } + } bool parsed_packet = false; - + // Try to get a new message if (mavlink_parse_char(chan, c, &msg, &status)) { hal.util->perf_begin(_perf_packet); packetReceived(status, msg); hal.util->perf_end(_perf_packet); parsed_packet = true; + gcs_alternative_active[chan] = false; + alternative.last_mavlink_ms = now_ms; } if (parsed_packet || i % 100 == 0) { diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp index 12f5a753fe..f442c98eeb 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp @@ -37,6 +37,7 @@ This provides some support code and variables for MAVLink enabled sketches #endif AP_HAL::UARTDriver *mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS]; +bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS]; mavlink_system_t mavlink_system = {7,1}; @@ -141,6 +142,10 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len) if (!valid_channel(chan)) { return; } + if (gcs_alternative_active[chan]) { + // an alternative protocol is active + return; + } mavlink_comm_port[chan]->write(buf, len); } diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h index dddab43896..410cc73993 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.h +++ b/libraries/GCS_MAVLink/GCS_MAVLink.h @@ -36,6 +36,7 @@ /// MAVLink stream used for uartA extern AP_HAL::UARTDriver *mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS]; +extern bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS]; /// MAVLink system definition extern mavlink_system_t mavlink_system;