From 5ef20aec27412d3b67d5ffb7b871109f051a9c36 Mon Sep 17 00:00:00 2001 From: yaapu Date: Thu, 10 Dec 2020 10:28:41 +0100 Subject: [PATCH] AP_RCProtocol: added support for passthrough telemetry over crossfire --- .../AP_RCProtocol/AP_RCProtocol_CRSF.cpp | 32 ++++--------------- libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h | 32 +++++++++++++++++-- .../AP_RCProtocol/AP_RCProtocol_FPort.cpp | 17 +++++----- libraries/AP_RCProtocol/AP_RCProtocol_FPort.h | 6 ++-- .../AP_RCProtocol/AP_RCProtocol_FPort2.cpp | 17 +++++----- .../AP_RCProtocol/AP_RCProtocol_FPort2.h | 6 ++-- 6 files changed, 61 insertions(+), 49 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index 12106fee08..c1fd607f33 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -187,12 +187,6 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) if (_frame_ofs == _frame.length + CSRF_HEADER_LEN) { log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame_ofs - CSRF_HEADER_LEN); - if ((timestamp_us - _last_frame_time_us) <= CRSF_INTER_FRAME_TIME_US_150HZ + CRSF_MAX_FRAME_TIME_US) { - _fast_telem = true; - } else { - _fast_telem = false; - } - // we consumed the partial frame, reset _frame_ofs = 0; @@ -209,9 +203,9 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) _last_frame_time_us = timestamp_us; // decode here if (decode_csrf_packet()) { - add_input(MAX_CHANNELS, _channels, false, _current_rssi); + add_input(MAX_CHANNELS, _channels, false, _link_status.rssi); } - } + } } void AP_RCProtocol_CRSF::update(void) @@ -328,19 +322,6 @@ bool AP_RCProtocol_CRSF::process_telemetry(bool check_constraint) return false; #endif } - /* - check that we haven't been too slow in responding to the new - UART data. If we respond too late then we will corrupt the next - incoming control frame - */ - uint64_t tend = uart->receive_time_constraint_us(1); - uint64_t now = AP_HAL::micros64(); - uint64_t tdelay = now - tend; - if (tdelay > CRSF_MAX_FRAME_TIME_US && check_constraint) { - // we've been too slow in responding - return false; - } - write_frame(&_telemetry_frame); // get fresh telem_data in the next call telem_available = false; @@ -361,13 +342,15 @@ void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data) } // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link if (rssi_dbm < 50) { - _current_rssi = 255; + _link_status.rssi = 255; } else if (rssi_dbm > 120) { - _current_rssi = 0; + _link_status.rssi = 0; } else { // this is an approximation recommended by Remo from TBS - _current_rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f)); + _link_status.rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f)); } + + _link_status.rf_mode = static_cast(MIN(link->rf_mode, 3U)); } // process a byte provided by a uart @@ -397,4 +380,3 @@ namespace AP { return AP_RCProtocol_CRSF::get_singleton(); } }; - diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index d2b7c32299..18aad41d65 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -55,6 +55,9 @@ public: CRSF_FRAMETYPE_PARAMETER_READ = 0x2C, CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D, CRSF_FRAMETYPE_COMMAND = 0x32, + // Custom Telemetry Frames 0x7F,0x80 + CRSF_FRAMETYPE_AP_CUSTOM_TELEM_LEGACY = 0x7F, // as suggested by Remo Masina for fw < 4.06 + CRSF_FRAMETYPE_AP_CUSTOM_TELEM = 0x80, // reserved for ArduPilot by TBS, requires fw >= 4.06 }; // Command IDs for CRSF_FRAMETYPE_COMMAND @@ -116,6 +119,13 @@ public: CRSF_COMMAND_RX_BIND = 0x01, }; + // SubType IDs for CRSF_FRAMETYPE_CUSTOM_TELEM + enum CustomTelemSubTypeID : uint8_t { + CRSF_AP_CUSTOM_TELEM_SINGLE_PACKET_PASSTHROUGH = 0xF0, + CRSF_AP_CUSTOM_TELEM_STATUS_TEXT = 0xF1, + CRSF_AP_CUSTOM_TELEM_MULTI_PACKET_PASSTHROUGH = 0xF2, + }; + enum DeviceAddress { CRSF_ADDRESS_BROADCAST = 0x00, CRSF_ADDRESS_USB = 0x10, @@ -161,6 +171,24 @@ public: int8_t downlink_dnr; // ( db ) } PACKED; + enum class RFMode : uint8_t { + CRSF_RF_MODE_4HZ = 0, + CRSF_RF_MODE_50HZ, + CRSF_RF_MODE_150HZ, + CRSF_RF_MODE_UNKNOWN, + }; + + struct LinkStatus { + int16_t rssi = -1; + RFMode rf_mode; + }; + + // this will be used by AP_CRSF_Telem to access link status data + // from within AP_RCProtocol_CRSF thread so no need for cross-thread synch + const volatile LinkStatus& get_link_status() const { + return _link_status; + } + private: struct Frame _frame; struct Frame _telemetry_frame; @@ -187,8 +215,8 @@ private: uint32_t _last_rx_time_us; uint32_t _start_frame_time_us; bool telem_available; - bool _fast_telem; // is 150Hz telemetry active - int16_t _current_rssi = -1; + + volatile struct LinkStatus _link_status; AP_HAL::UARTDriver *_uart; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp index 2e7c75906a..e22b01e4cd 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp @@ -148,11 +148,12 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame) status text messages */ if (!telem_data.available) { - if (!AP_Frsky_Telem::get_telem_data(telem_data.frame, telem_data.appid, telem_data.data)) { + uint8_t packet_count; + if (!AP_Frsky_Telem::get_telem_data(&telem_data.packet, packet_count, 1)) { // nothing to send, send a null frame - telem_data.frame = 0x00; - telem_data.appid = 0x00; - telem_data.data = 0x00; + telem_data.packet.frame = 0x00; + telem_data.packet.appid = 0x00; + telem_data.packet.data = 0x00; } telem_data.available = true; } @@ -172,10 +173,10 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame) buf[0] = 0x08; buf[1] = 0x81; - buf[2] = telem_data.frame; - buf[3] = telem_data.appid & 0xFF; - buf[4] = telem_data.appid >> 8; - memcpy(&buf[5], &telem_data.data, 4); + buf[2] = telem_data.packet.frame; + buf[3] = telem_data.packet.appid & 0xFF; + buf[4] = telem_data.packet.appid >> 8; + memcpy(&buf[5], &telem_data.packet.data, 4); buf[9] = crc_sum8(&buf[0], 9); // perform byte stuffing per FPort spec diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.h b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.h index a971f5ba1b..0e6a3dd6d5 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.h @@ -17,6 +17,8 @@ #pragma once +#include + #include "AP_RCProtocol.h" #include "SoftSerial.h" @@ -50,9 +52,7 @@ private: struct { bool available = false; - uint32_t data; - uint16_t appid; - uint8_t frame; + AP_Frsky_SPort::sport_packet_t packet; } telem_data; // receiver sends 0x10 when ready to receive telemetry frames (R-XSR) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp index 27910fb3c5..c314138f54 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp @@ -137,10 +137,11 @@ void AP_RCProtocol_FPort2::decode_downlink(const FPort2_Frame &frame) send it in the next call, this prevents corruption of status text messages */ if (!telem_data.available) { - if (!AP_Frsky_Telem::get_telem_data(telem_data.frame, telem_data.appid, telem_data.data)) { - telem_data.frame = 0x00; - telem_data.appid = 0x00; - telem_data.data = 0x00; + uint8_t packet_count; + if (!AP_Frsky_Telem::get_telem_data(&telem_data.packet, packet_count, 1)) { + telem_data.packet.frame = 0x00; + telem_data.packet.appid = 0x00; + telem_data.packet.data = 0x00; } telem_data.available = true; } @@ -169,10 +170,10 @@ void AP_RCProtocol_FPort2::decode_downlink(const FPort2_Frame &frame) buf[1] = frame.type; // do not consume telemetry data for invalid downlink frames, i.e. incoming prim == 0x00 if (frame.downlink.prim != FPORT2_PRIM_NULL) { - buf[2] = telem_data.frame; - buf[3] = telem_data.appid & 0xFF; - buf[4] = telem_data.appid >> 8; - memcpy(&buf[5], &telem_data.data, 4); + buf[2] = telem_data.packet.frame; + buf[3] = telem_data.packet.appid & 0xFF; + buf[4] = telem_data.packet.appid >> 8; + memcpy(&buf[5], &telem_data.packet.data, 4); // get fresh telem_data in the next call telem_data.available = false; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.h b/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.h index b7fff899c1..e7cfd5fd9e 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.h @@ -17,6 +17,8 @@ #pragma once +#include + #include "AP_RCProtocol.h" #include "SoftSerial.h" @@ -53,8 +55,6 @@ private: struct { bool available; - uint32_t data; - uint16_t appid; - uint8_t frame; + AP_Frsky_SPort::sport_packet_t packet; } telem_data; };