From 24382e25fc0ff2e880e931102ccf59f59d6f4896 Mon Sep 17 00:00:00 2001 From: yaapu Date: Sun, 20 Dec 2020 20:57:08 +0100 Subject: [PATCH] AP_Frsky_Telem: added support for sending frsky telemetry data from scripting --- libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp | 102 +++++++++++++++++ libraries/AP_Frsky_Telem/AP_Frsky_SPort.h | 30 ++++- .../AP_Frsky_SPort_Passthrough.cpp | 105 ++++++------------ .../AP_Frsky_SPort_Passthrough.h | 9 +- 4 files changed, 168 insertions(+), 78 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp index 7d5c56b9e5..31889f1b99 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp @@ -9,6 +9,8 @@ #include +AP_Frsky_SPort *AP_Frsky_SPort::singleton; + /* * send telemetry data * for FrSky SPort protocol (X-receivers) @@ -125,6 +127,16 @@ void AP_Frsky_SPort::send(void) _SPort.various_call = 0; } break; + default: + { + // respond to custom user data polling + WITH_SEMAPHORE(_sport_push_buffer.sem); + if (_sport_push_buffer.pending && readbyte == _sport_push_buffer.packet.sensor) { + send_sport_frame(_sport_push_buffer.packet.frame, _sport_push_buffer.packet.appid, _sport_push_buffer.packet.data); + _sport_push_buffer.pending = false; + } + } + break; } _SPort.sport_status = false; } @@ -343,3 +355,93 @@ uint8_t AP_Frsky_SPort::calc_sensor_id(const uint8_t physical_id) result += (BIT(physical_id, 0) ^ BIT(physical_id, 2) ^ BIT(physical_id, 4)) << 7; return result; } + +/* + * prepare value for transmission through FrSky link + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +uint16_t AP_Frsky_SPort::prep_number(int32_t number, uint8_t digits, uint8_t power) +{ + uint16_t res = 0; + uint32_t abs_number = abs(number); + + if ((digits == 2) && (power == 1)) { // number encoded on 8 bits: 7 bits for digits + 1 for 10^power + if (abs_number < 100) { + res = abs_number<<1; + } else if (abs_number < 1270) { + res = ((uint8_t)roundf(abs_number * 0.1f)<<1)|0x1; + } else { // transmit max possible value (0x7F x 10^1 = 1270) + res = 0xFF; + } + if (number < 0) { // if number is negative, add sign bit in front + res |= 0x1<<8; + } + } else if ((digits == 2) && (power == 2)) { // number encoded on 9 bits: 7 bits for digits + 2 for 10^power + if (abs_number < 100) { + res = abs_number<<2; + } else if (abs_number < 1000) { + res = ((uint8_t)roundf(abs_number * 0.1f)<<2)|0x1; + } else if (abs_number < 10000) { + res = ((uint8_t)roundf(abs_number * 0.01f)<<2)|0x2; + } else if (abs_number < 127000) { + res = ((uint8_t)roundf(abs_number * 0.001f)<<2)|0x3; + } else { // transmit max possible value (0x7F x 10^3 = 127000) + res = 0x1FF; + } + if (number < 0) { // if number is negative, add sign bit in front + res |= 0x1<<9; + } + } else if ((digits == 3) && (power == 1)) { // number encoded on 11 bits: 10 bits for digits + 1 for 10^power + if (abs_number < 1000) { + res = abs_number<<1; + } else if (abs_number < 10240) { + res = ((uint16_t)roundf(abs_number * 0.1f)<<1)|0x1; + } else { // transmit max possible value (0x3FF x 10^1 = 10240) + res = 0x7FF; + } + if (number < 0) { // if number is negative, add sign bit in front + res |= 0x1<<11; + } + } else if ((digits == 3) && (power == 2)) { // number encoded on 12 bits: 10 bits for digits + 2 for 10^power + if (abs_number < 1000) { + res = abs_number<<2; + } else if (abs_number < 10000) { + res = ((uint16_t)roundf(abs_number * 0.1f)<<2)|0x1; + } else if (abs_number < 100000) { + res = ((uint16_t)roundf(abs_number * 0.01f)<<2)|0x2; + } else if (abs_number < 1024000) { + res = ((uint16_t)roundf(abs_number * 0.001f)<<2)|0x3; + } else { // transmit max possible value (0x3FF x 10^3 = 127000) + res = 0xFFF; + } + if (number < 0) { // if number is negative, add sign bit in front + res |= 0x1<<12; + } + } + return res; +} + +/* + * Push user data down the telemetry link by responding to sensor polling (sport) + * or by using dedicated slots in the scheduler (fport) + * for SPort and FPort protocols (X-receivers) + */ +bool AP_Frsky_SPort::sport_telemetry_push(uint8_t sensor, uint8_t frame, uint16_t appid, int32_t data) +{ + WITH_SEMAPHORE(_sport_push_buffer.sem); + if (_sport_push_buffer.pending) { + return false; + } + _sport_push_buffer.packet.sensor = sensor; + _sport_push_buffer.packet.frame = frame; + _sport_push_buffer.packet.appid = appid; + _sport_push_buffer.packet.data = static_cast(data); + _sport_push_buffer.pending = true; + return true; +} + +namespace AP { + AP_Frsky_SPort *frsky_sport() { + return AP_Frsky_SPort::get_singleton(); + } +}; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h index 79797252f8..ddc72474ec 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h @@ -7,9 +7,23 @@ class AP_Frsky_SPort : public AP_Frsky_Backend public: - using AP_Frsky_Backend::AP_Frsky_Backend; + AP_Frsky_SPort(AP_HAL::UARTDriver *port) : AP_Frsky_Backend(port) { + singleton = this; + } + + /* Do not allow copies */ + AP_Frsky_SPort(const AP_Frsky_SPort &other) = delete; + AP_Frsky_SPort &operator=(const AP_Frsky_SPort&) = delete; void send() override; + // send an sport packet by responding to the specified polled sensor + bool sport_telemetry_push(const uint8_t sensor, const uint8_t frame, const uint16_t appid, const int32_t data); + // utility method to pack numbers in a compact size + uint16_t prep_number(int32_t const number, const uint8_t digits, const uint8_t power); + + static AP_Frsky_SPort *get_singleton(void) { + return singleton; + } protected: @@ -22,9 +36,14 @@ protected: } _passthrough; uint32_t calc_gps_latlng(bool &send_latitude); - static uint8_t calc_sensor_id(const uint8_t physical_id); + struct { + sport_packet_t packet; + bool pending = false; + HAL_Semaphore sem; + } _sport_push_buffer; + private: struct { @@ -36,4 +55,11 @@ private: uint8_t vario_call; uint8_t various_call; } _SPort; + + static AP_Frsky_SPort *singleton; + +}; + +namespace AP { + AP_Frsky_SPort *frsky_sport(); }; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index 724a9c615e..95d1b58143 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -110,6 +110,7 @@ void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void) set_scheduler_entry(BATT_2, 1300, 500); // 0x5008 Battery 2 status set_scheduler_entry(BATT_1, 1300, 500); // 0x5003 Battery 1 status set_scheduler_entry(PARAM, 1700, 1000); // 0x5007 parameters + set_scheduler_entry(UDATA, 5000, 200); // user data #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL set_scheduler_entry(MAV, 35, 25); // mavlite // initialize sport sensor IDs @@ -160,6 +161,12 @@ void AP_Frsky_SPort_Passthrough::adjust_packet_weight(bool queue_empty) _scheduler.packet_weight[ATTITUDE] = 45; // attitude } #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + // when using fport raise user data priority if any packets are pending + if (_use_external_data && _sport_push_buffer.pending) { + _scheduler.packet_weight[UDATA] = 250; + } else { + _scheduler.packet_weight[UDATA] = 5000; // user data + } } // WFQ scheduler @@ -181,10 +188,15 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty) case BATT_2: packet_ready = AP::battery().num_instances() > 1; break; + case UDATA: + // when using fport user data is sent by scheduler + // when using sport user data is sent responding to custom polling + packet_ready = _use_external_data && _sport_push_buffer.pending; + break; #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL - case MAV: - packet_ready = !_SPort_bidir.tx_packet_queue.is_empty(); - break; + case MAV: + packet_ready = !_SPort_bidir.tx_packet_queue.is_empty(); + break; #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL default: packet_ready = true; @@ -242,6 +254,15 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx) case PARAM: // 0x5007 parameters send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param()); break; + case UDATA: // user data + { + WITH_SEMAPHORE(_sport_push_buffer.sem); + if (_use_external_data && _sport_push_buffer.pending) { + send_sport_frame(_sport_push_buffer.packet.frame, _sport_push_buffer.packet.appid, _sport_push_buffer.packet.data); + _sport_push_buffer.pending = false; + } + } + break; #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL case MAV: // mavlite process_tx_queue(); @@ -276,8 +297,17 @@ void AP_Frsky_SPort_Passthrough::send(void) #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL } // check if we should respond to this polling byte - if (prev_byte == FRAME_HEAD && is_passthrough_byte(_passthrough.new_byte)) { - run_wfq_scheduler(); + if (prev_byte == FRAME_HEAD) { + if (is_passthrough_byte(_passthrough.new_byte)) { + run_wfq_scheduler(); + } else { + // respond to custom user data polling + WITH_SEMAPHORE(_sport_push_buffer.sem); + if (_sport_push_buffer.pending && _passthrough.new_byte == _sport_push_buffer.packet.sensor) { + send_sport_frame(_sport_push_buffer.packet.frame, _sport_push_buffer.packet.appid, _sport_push_buffer.packet.data); + _sport_push_buffer.pending = false; + } + } } } @@ -597,71 +627,6 @@ bool AP_Frsky_SPort_Passthrough::get_telem_data(sport_packet_t* packet_array, ui return idx > 0; } -/* - * prepare value for transmission through FrSky link - * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) - */ -uint16_t AP_Frsky_SPort_Passthrough::prep_number(int32_t number, uint8_t digits, uint8_t power) -{ - uint16_t res = 0; - uint32_t abs_number = abs(number); - - if ((digits == 2) && (power == 1)) { // number encoded on 8 bits: 7 bits for digits + 1 for 10^power - if (abs_number < 100) { - res = abs_number<<1; - } else if (abs_number < 1270) { - res = ((uint8_t)roundf(abs_number * 0.1f)<<1)|0x1; - } else { // transmit max possible value (0x7F x 10^1 = 1270) - res = 0xFF; - } - if (number < 0) { // if number is negative, add sign bit in front - res |= 0x1<<8; - } - } else if ((digits == 2) && (power == 2)) { // number encoded on 9 bits: 7 bits for digits + 2 for 10^power - if (abs_number < 100) { - res = abs_number<<2; - } else if (abs_number < 1000) { - res = ((uint8_t)roundf(abs_number * 0.1f)<<2)|0x1; - } else if (abs_number < 10000) { - res = ((uint8_t)roundf(abs_number * 0.01f)<<2)|0x2; - } else if (abs_number < 127000) { - res = ((uint8_t)roundf(abs_number * 0.001f)<<2)|0x3; - } else { // transmit max possible value (0x7F x 10^3 = 127000) - res = 0x1FF; - } - if (number < 0) { // if number is negative, add sign bit in front - res |= 0x1<<9; - } - } else if ((digits == 3) && (power == 1)) { // number encoded on 11 bits: 10 bits for digits + 1 for 10^power - if (abs_number < 1000) { - res = abs_number<<1; - } else if (abs_number < 10240) { - res = ((uint16_t)roundf(abs_number * 0.1f)<<1)|0x1; - } else { // transmit max possible value (0x3FF x 10^1 = 10240) - res = 0x7FF; - } - if (number < 0) { // if number is negative, add sign bit in front - res |= 0x1<<11; - } - } else if ((digits == 3) && (power == 2)) { // number encoded on 12 bits: 10 bits for digits + 2 for 10^power - if (abs_number < 1000) { - res = abs_number<<2; - } else if (abs_number < 10000) { - res = ((uint16_t)roundf(abs_number * 0.1f)<<2)|0x1; - } else if (abs_number < 100000) { - res = ((uint16_t)roundf(abs_number * 0.01f)<<2)|0x2; - } else if (abs_number < 1024000) { - res = ((uint16_t)roundf(abs_number * 0.001f)<<2)|0x3; - } else { // transmit max possible value (0x3FF x 10^3 = 127000) - res = 0xFFF; - } - if (number < 0) { // if number is negative, add sign bit in front - res |= 0x1<<12; - } - } - return res; -} - #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL /* allow external transports (e.g. FPort), to supply telemetry data diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h index 99188f0a30..ce0f9c2014 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h @@ -13,10 +13,7 @@ #include "AP_Frsky_MAVlite_MAVliteToSPort.h" #include "AP_Frsky_MAVliteMsgHandler.h" -#define FRSKY_WFQ_TIME_SLOT_MAX 12U #define SPORT_TX_PACKET_DUPLICATES 1 // number of duplicates packets we send (fport only) -#else -#define FRSKY_WFQ_TIME_SLOT_MAX 11U #endif class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry @@ -25,7 +22,7 @@ public: AP_Frsky_SPort_Passthrough(AP_HAL::UARTDriver *port, bool use_external_data, AP_Frsky_Parameters *&frsky_parameters) : AP_Frsky_SPort(port), - AP_RCTelemetry(FRSKY_WFQ_TIME_SLOT_MAX), + AP_RCTelemetry(WFQ_LAST_ITEM), _use_external_data(use_external_data), _frsky_parameters(frsky_parameters) { @@ -70,8 +67,9 @@ public: BATT_2 = 8, // 0x5008 Battery 2 status BATT_1 = 9, // 0x5008 Battery 1 status PARAM = 10, // 0x5007 parameters + UDATA = 11, // user data #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL - MAV = 11, // mavlite + MAV = 12, // mavlite #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL WFQ_LAST_ITEM // must be last }; @@ -151,7 +149,6 @@ private: uint8_t _paramID; uint32_t calc_gps_status(void); - uint16_t prep_number(int32_t number, uint8_t digits, uint8_t power); static AP_Frsky_SPort_Passthrough *singleton; };