From e044cdfde792fb7c0c06e36bd3cee3830c9b9d13 Mon Sep 17 00:00:00 2001 From: yaapu Date: Wed, 18 Nov 2020 18:16:15 +0100 Subject: [PATCH] AP_Frsky_Telem: fix for conditional compilation of bidir support --- libraries/AP_Frsky_Telem/AP_Frsky_Backend.h | 7 ++++ libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.h | 2 +- .../AP_Frsky_MAVliteMsgHandler.cpp | 2 + .../AP_Frsky_MAVlite_MAVliteToSPort.cpp | 2 + .../AP_Frsky_MAVlite_MAVliteToSPort.h | 2 + .../AP_Frsky_MAVlite_Message.cpp | 2 + .../AP_Frsky_Telem/AP_Frsky_MAVlite_Message.h | 2 + .../AP_Frsky_MAVlite_SPortToMAVlite.cpp | 2 + .../AP_Frsky_MAVlite_SPortToMAVlite.h | 2 + .../AP_Frsky_SPort_Passthrough.cpp | 41 +++++++++---------- .../AP_Frsky_SPort_Passthrough.h | 2 + libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 4 ++ libraries/AP_Frsky_Telem/AP_Frsky_Telem.h | 7 +--- 13 files changed, 48 insertions(+), 29 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h index 066b4a2ce9..4ac9136ef7 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h @@ -3,6 +3,10 @@ #include #include +#ifndef HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL +#define HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL 1 +#endif + class AP_Frsky_Backend { public: @@ -26,10 +30,13 @@ public: { return false; } + +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL virtual bool set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) { return false; } +#endif virtual void queue_text_message(MAV_SEVERITY severity, const char *text) { } diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.h b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.h index 03af24b5e0..f5e2e7d8fe 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite.h @@ -1,4 +1,5 @@ #pragma once +#include "AP_Frsky_Backend.h" /* @@ -26,7 +27,6 @@ */ - #define MAVLITE_MAX_PAYLOAD_LEN 31 // 7 float params + cmd_id + options #define MAVLITE_MSG_SPORT_PACKETS_COUNT(LEN) static_cast(1 + ceilf((LEN-2)/5.0f)) // number of sport packets required to transport a message with LEN payload #define SPORT_PACKET_QUEUE_LENGTH static_cast(30U*MAVLITE_MSG_SPORT_PACKETS_COUNT(MAVLITE_MAX_PAYLOAD_LEN)) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp index 532810d1a4..b6bc0e0f19 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp @@ -5,6 +5,7 @@ extern const AP_HAL::HAL& hal; +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL /* * Handle the COMMAND_LONG mavlite message * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) @@ -284,3 +285,4 @@ bool AP_Frsky_MAVliteMsgHandler::send_message(AP_Frsky_MAVlite_Message &txmsg) { return _send_fn(txmsg); } +#endif \ No newline at end of file diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.cpp index 32c820d1a9..31630ce881 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.cpp @@ -4,6 +4,7 @@ #include "AP_Frsky_SPort.h" +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL void AP_Frsky_MAVlite_MAVliteToSPort::reset() { checksum = 0; @@ -96,3 +97,4 @@ void AP_Frsky_MAVlite_MAVliteToSPort::process_byte(const uint8_t b, ObjectBuffer packet_offs = 2; // skip setting sensorid and frame } } +#endif \ No newline at end of file diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.h b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.h index 0efbf254de..64144b86ff 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_MAVliteToSPort.h @@ -7,6 +7,7 @@ #include +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL /* * An instance of this class encodes a MAVlite message into several * SPort packets, and pushes them onto the supplied queue. @@ -50,3 +51,4 @@ private: int16_t checksum; // sent at end of packet void update_checksum(const uint8_t c); }; +#endif \ No newline at end of file diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_Message.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_Message.cpp index 7ada1d68e7..5d35624ece 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_Message.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_Message.cpp @@ -2,6 +2,7 @@ #include +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL bool AP_Frsky_MAVlite_Message::get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const { if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) { @@ -53,3 +54,4 @@ void AP_Frsky_MAVlite_Message::bit8_pack(uint8_t &value, const uint8_t bit_value } value |= (bit_value< +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL class AP_Frsky_MAVlite_Message { public: // helpers @@ -44,3 +45,4 @@ private: bool get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const WARN_IF_UNUSED; bool set_bytes(const uint8_t *bytes, const uint8_t offset, const uint8_t count) WARN_IF_UNUSED; }; +#endif \ No newline at end of file diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.cpp index 73309e30f5..26b8b5b2af 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.cpp @@ -2,6 +2,7 @@ #include "AP_Frsky_MAVlite.h" +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL void AP_Frsky_MAVlite_SPortToMAVlite::reset(void) { checksum = 0; @@ -102,3 +103,4 @@ void AP_Frsky_MAVlite_SPortToMAVlite::parse(uint8_t byte) return; } } +#endif \ No newline at end of file diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.h b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.h index 9a2e981172..36d7bf8c52 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_SPortToMAVlite.h @@ -5,6 +5,7 @@ #include +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL /* * An instance of this class decodes a stream of SPort packets into a * MAVlite message (see AP_Frsky_MAVlite_Message.h). It is expected @@ -46,3 +47,4 @@ private: int16_t checksum; // sent at end of packet void update_checksum(const uint8_t c); }; +#endif \ No newline at end of file diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index 0ba6eb01c4..8aa21f2f93 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -545,27 +545,6 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void) return attiandrng; } -#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL -/* - allow external transports (e.g. FPort), to supply telemetry data - */ -bool AP_Frsky_SPort_Passthrough::set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) -{ - // queue only Uplink packets - if (frame == SPORT_UPLINK_FRAME || frame == SPORT_UPLINK_FRAME_RW) { - const AP_Frsky_SPort::sport_packet_t sp { - 0x00, // this is ignored by process_sport_rx_queue() so no need for a real sensor ID - frame, - appid, - data - }; - - _SPort_bidir.rx_packet_queue.push_force(sp); - return true; - } - return false; -} - /* fetch Sport data for an external transport, such as FPort */ @@ -647,8 +626,26 @@ uint16_t AP_Frsky_SPort_Passthrough::prep_number(int32_t number, uint8_t digits, return res; } +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL +/* + allow external transports (e.g. FPort), to supply telemetry data + */ +bool AP_Frsky_SPort_Passthrough::set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) +{ + // queue only Uplink packets + if (frame == SPORT_UPLINK_FRAME || frame == SPORT_UPLINK_FRAME_RW) { + const AP_Frsky_SPort::sport_packet_t sp { + 0x00, // this is ignored by process_sport_rx_queue() so no need for a real sensor ID + frame, + appid, + data + }; - + _SPort_bidir.rx_packet_queue.push_force(sp); + return true; + } + return false; +} /* * Queue uplink packets in the sport rx queue diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h index 18a9eab0aa..2994e2b7c6 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h @@ -43,7 +43,9 @@ public: bool get_next_msg_chunk(void) override; bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) override; +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL bool set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) override; +#endif void queue_text_message(MAV_SEVERITY severity, const char *text) override { diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index b840eef67b..bbb27ab45a 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -89,6 +89,7 @@ bool AP_Frsky_Telem::_get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t & return _backend->get_telem_data(frame, appid, data); } +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL bool AP_Frsky_Telem::_set_telem_data(uint8_t frame, uint16_t appid, uint32_t data) { if (_backend == nullptr) { @@ -96,6 +97,7 @@ bool AP_Frsky_Telem::_set_telem_data(uint8_t frame, uint16_t appid, uint32_t dat } return _backend->set_telem_data(frame, appid, data); } +#endif /* fetch Sport data for an external transport, such as FPort @@ -117,6 +119,7 @@ bool AP_Frsky_Telem::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &d return singleton->_get_telem_data(frame, appid, data); } +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL /* allow external transports (e.g. FPort), to supply telemetry data */ @@ -135,6 +138,7 @@ bool AP_Frsky_Telem::set_telem_data(const uint8_t frame, const uint16_t appid, c } return singleton->_set_telem_data(frame, appid, data); } +#endif namespace AP { diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index 3d580b67c5..ecdf9f6bb7 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -14,10 +14,6 @@ */ #pragma once -#ifndef HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL -#define HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL 1 -#endif - #include "AP_Frsky_Backend.h" class AP_Frsky_Parameters; @@ -57,12 +53,11 @@ public: private: AP_Frsky_Backend *_backend; - -#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL AP_Frsky_Parameters* _frsky_parameters; // get next telemetry data for external consumers of SPort data (internal function) bool _get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data); +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL // set telemetry data from external producer of SPort data (internal function) bool _set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data); #endif