From 19c6b0b8aea8332e9f49fb1533375adf2d32d1eb Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 13 Dec 2023 08:47:51 +0000 Subject: [PATCH] AP_RCProtocol: IRC Ghost protocol --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 12 + libraries/AP_RCProtocol/AP_RCProtocol.h | 6 + .../AP_RCProtocol/AP_RCProtocol_GHST.cpp | 434 ++++++++++++++++++ libraries/AP_RCProtocol/AP_RCProtocol_GHST.h | 198 ++++++++ .../AP_RCProtocol/AP_RCProtocol_config.h | 4 + 5 files changed, 654 insertions(+) create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_GHST.h diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 9c8ef679a1..9d9174e89f 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -33,6 +33,7 @@ #include "AP_RCProtocol_FPort.h" #include "AP_RCProtocol_FPort2.h" #include "AP_RCProtocol_DroneCAN.h" +#include "AP_RCProtocol_GHST.h" #include #include @@ -82,6 +83,9 @@ void AP_RCProtocol::init() #if AP_RCPROTOCOL_DRONECAN_ENABLED backend[AP_RCProtocol::DRONECAN] = new AP_RCProtocol_DroneCAN(*this); #endif +#if AP_RCPROTOCOL_GHST_ENABLED + backend[AP_RCProtocol::GHST] = new AP_RCProtocol_GHST(*this); +#endif } AP_RCProtocol::~AP_RCProtocol() @@ -303,6 +307,10 @@ static const AP_RCProtocol::SerialConfig serial_configs[] { // CRSFv3 can negotiate higher rates which are sticky on soft reboot { 2000000, 0, 1, false }, #endif +#if AP_RCPROTOCOL_GHST_ENABLED + // Ghost: + { 420000, 0, 1, false }, +#endif }; static_assert(ARRAY_SIZE(serial_configs) > 1, "must have at least one serial config"); @@ -504,6 +512,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) #if AP_RCPROTOCOL_DRONECAN_ENABLED case DRONECAN: return "DroneCAN"; +#endif +#if AP_RCPROTOCOL_GHST_ENABLED + case GHST: + return "GHST"; #endif case NONE: break; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index eeacb6fa0a..8889fdce89 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -69,6 +69,9 @@ public: #endif #if AP_RCPROTOCOL_DRONECAN_ENABLED DRONECAN = 13, +#endif +#if AP_RCPROTOCOL_GHST_ENABLED + GHST = 14, #endif NONE //last enum always is None }; @@ -129,6 +132,9 @@ public: #endif #if AP_RCPROTOCOL_CRSF_ENABLED case CRSF: +#endif +#if AP_RCPROTOCOL_GHST_ENABLED + case GHST: #endif return true; #if AP_RCPROTOCOL_IBUS_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp new file mode 100644 index 0000000000..1f2fa06950 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp @@ -0,0 +1,434 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + GHST protocol decoder based on betaflight implementation + Code by Andy Piper + */ + +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_GHST_ENABLED + +#include "AP_RCProtocol.h" +#include "AP_RCProtocol_GHST.h" +#include +#include +#include +#include +#include +#include + +/* + * GHST protocol + * + * GHST protocol uses a single wire half duplex uart connection. + * + * 420000 baud + * not inverted + * 8 Bit + * 1 Stop bit + * Big endian + * Max frame size is 14 bytes + * + * Every frame has the structure: + * + * + * Device address: (uint8_t) + * Frame length: length in bytes including Type (uint8_t) + * Type: (uint8_t) + * CRC: (uint8_t) + * + */ + +extern const AP_HAL::HAL& hal; + +//#define GHST_DEBUG +//#define GHST_DEBUG_CHARS +#ifdef GHST_DEBUG +# define debug(fmt, args...) hal.console->printf("GHST: " fmt "\n", ##args) +static const char* get_frame_type(uint8_t byte, uint8_t subtype = 0) +{ + switch(byte) { + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_5TO8: + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_12_5TO8: + return "RC5_8"; + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_9TO12: + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_12_9TO12: + return "RC9_12"; + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_13TO16: + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_12_13TO16: + return "RC13_16"; + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_RSSI: + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_12_RSSI: + return "RSSI"; + case AP_RCProtocol_GHST::GHST_UL_RC_VTX_CTRL: + return "VTX_CTRL"; + case AP_RCProtocol_GHST::GHST_UL_VTX_SETUP: + return "VTX_SETUP"; + } + return "UNKNOWN"; +} +#else +# define debug(fmt, args...) do {} while(0) +#endif + +#define GHST_MAX_FRAME_TIME_US 500U // 14 bytes @ 420k = ~450us +#define GHST_FRAME_TIMEOUT_US 10000U // 10ms to account for scheduling delays +#define GHST_INTER_FRAME_TIME_US 2000U // At fastest, frames are sent by the transmitter every 2 ms, 500 Hz +#define GHST_HEADER_TYPE_LEN (GHST_HEADER_LEN + 1) // header length including type + +const uint16_t AP_RCProtocol_GHST::RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES] = { + 55, 160, 250, 19, 250, 500, 150, 250, +}; + +AP_RCProtocol_GHST* AP_RCProtocol_GHST::_singleton; + +AP_RCProtocol_GHST::AP_RCProtocol_GHST(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) +{ +#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) + if (_singleton != nullptr) { + AP_HAL::panic("Duplicate GHST handler"); + } + + _singleton = this; +#else + if (_singleton == nullptr) { + _singleton = this; + } +#endif +} + +AP_RCProtocol_GHST::~AP_RCProtocol_GHST() { + _singleton = nullptr; +} + +// get the protocol string +const char* AP_RCProtocol_GHST::get_protocol_string() const { + return "GHST"; +} + +// return the link rate as defined by the LinkStatistics +uint16_t AP_RCProtocol_GHST::get_link_rate() const { + return RF_MODE_RATES[_link_status.rf_mode - GHST_RF_MODE_NORMAL]; +} + +void AP_RCProtocol_GHST::_process_byte(uint32_t timestamp_us, uint8_t byte) +{ + //debug("process_byte(0x%x)", byte); + // we took too long decoding, start again - the RX will only send complete frames so this is unlikely to fail, + // however thread scheduling can introduce longer delays even when the data has been received + if (_frame_ofs > 0 && (timestamp_us - _start_frame_time_us) > GHST_FRAME_TIMEOUT_US) { + _frame_ofs = 0; + } + + // overflow check + if (_frame_ofs >= GHST_FRAMELEN_MAX) { + _frame_ofs = 0; + } + + // start of a new frame + if (_frame_ofs == 0) { + _start_frame_time_us = timestamp_us; + } + + add_to_buffer(_frame_ofs++, byte); + + // need a header to get the length + if (_frame_ofs < GHST_HEADER_TYPE_LEN) { + return; + } + + // parse the length + if (_frame_ofs == GHST_HEADER_TYPE_LEN) { + _frame_crc = crc8_dvb_s2(0, _frame.type); + // check for garbage frame + if (_frame.length > GHST_FRAME_PAYLOAD_MAX) { + _frame_ofs = 0; + } + return; + } + + // update crc + if (_frame_ofs < _frame.length + GHST_HEADER_LEN) { + _frame_crc = crc8_dvb_s2(_frame_crc, byte); + } + + // overflow check + if (_frame_ofs > _frame.length + GHST_HEADER_LEN) { + _frame_ofs = 0; + return; + } + + // decode whatever we got and expect + if (_frame_ofs == _frame.length + GHST_HEADER_LEN) { + log_data(AP_RCProtocol::GHST, timestamp_us, (const uint8_t*)&_frame, _frame_ofs - GHST_HEADER_LEN); + + // we consumed the partial frame, reset + _frame_ofs = 0; + + // bad CRC (payload start is +1 from frame start, so need to subtract that from frame length to get index) + if (_frame_crc != _frame.payload[_frame.length - 2]) { + return; + } + + _last_frame_time_us = _last_rx_frame_time_us = timestamp_us; + // decode here + if (decode_ghost_packet()) { + _last_tx_frame_time_us = timestamp_us; // we have received a frame from the transmitter + add_input(MAX_CHANNELS, _channels, false, _link_status.rssi, _link_status.link_quality); + } + } +} + +void AP_RCProtocol_GHST::update(void) +{ +} + +// write out a frame of any type +bool AP_RCProtocol_GHST::write_frame(Frame* frame) +{ + AP_HAL::UARTDriver *uart = get_current_UART(); + + if (!uart) { + return false; + } + + // 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. incoming packets at max 126bits @500Hz @420k baud gives total budget of 2ms + // per packet of which we need 300us to receive a packet. outgoing packets are 126bits which require 300us to send + // leaving at most 1.4ms of delay that can be tolerated + uint64_t tend = uart->receive_time_constraint_us(1); + uint64_t now = AP_HAL::micros64(); + uint64_t tdelay = now - tend; + if (tdelay > 1000) { + // we've been too slow in responding + return false; + } + + // calculate crc + uint8_t crc = crc8_dvb_s2(0, frame->type); + for (uint8_t i = 0; i < frame->length - 2; i++) { + crc = crc8_dvb_s2(crc, frame->payload[i]); + } + frame->payload[frame->length - 2] = crc; + + uart->write((uint8_t*)frame, frame->length + 2); + uart->flush(); + +#ifdef GHST_DEBUG + hal.console->printf("GHST: writing %s:", get_frame_type(frame->type, frame->payload[0])); + for (uint8_t i = 0; i < frame->length + 2; i++) { + uint8_t val = ((uint8_t*)frame)[i]; +#ifdef GHST_DEBUG_CHARS + if (val >= 32 && val <= 126) { + hal.console->printf(" 0x%x '%c'", val, (char)val); + } else { +#endif + hal.console->printf(" 0x%x", val); +#ifdef GHST_DEBUG_CHARS + } +#endif + } + hal.console->printf("\n"); +#endif + return true; +} + +bool AP_RCProtocol_GHST::decode_ghost_packet() +{ +#ifdef GHST_DEBUG + hal.console->printf("GHST: received %s:", get_frame_type(_frame.type)); + uint8_t* fptr = (uint8_t*)&_frame; + for (uint8_t i = 0; i < _frame.length + 2; i++) { +#ifdef GHST_DEBUG_CHARS + if (fptr[i] >= 32 && fptr[i] <= 126) { + hal.console->printf(" 0x%x '%c'", fptr[i], (char)fptr[i]); + } else { +#endif + hal.console->printf(" 0x%x", fptr[i]); +#ifdef GHST_DEBUG_CHARS + } +#endif + } + hal.console->printf("\n"); +#endif + + const RadioFrame* radio_frame = (const RadioFrame*)(&_frame.payload); + const Channels12Bit_4Chan* channels = &(radio_frame->channels); + const uint8_t* lowres_channels = radio_frame->lowres_channels; + + // Scaling from Betaflight + // Scaling 12bit channels (8bit channels in brackets) + // OpenTx RC PWM (BF) + // min -1024 0( 0) 988us + // ctr 0 2048(128) 1500us + // max 1024 4096(256) 2012us + // + + // Scaling legacy (nearly 10bit) + // derived from original SBus scaling, with slight correction for offset + // now symmetrical around OpenTx 0 value + // scaling is: + // OpenTx RC PWM (BF) + // min -1024 172( 22) 988us + // ctr 0 992(124) 1500us + // max 1024 1811(226) 2012us + +#define CHANNEL_RESCALE(x) (((5 * x) >> 2) - 430) +#define CHANNEL_SCALE(x) (int32_t(x) / 4 + 988) +#define CHANNEL_SCALE_LEGACY(x) CHANNEL_SCALE(CHANNEL_RESCALE(x)) + + // legacy scaling + if (_frame.type >= GHST_UL_RC_CHANS_HS4_5TO8 && _frame.type <= GHST_UL_RC_CHANS_RSSI) { + _channels[0] = CHANNEL_SCALE_LEGACY(channels->ch0); + _channels[1] = CHANNEL_SCALE_LEGACY(channels->ch1); + _channels[2] = CHANNEL_SCALE_LEGACY(channels->ch2); + _channels[3] = CHANNEL_SCALE_LEGACY(channels->ch3); + } else { + _channels[0] = CHANNEL_SCALE(channels->ch0); + _channels[1] = CHANNEL_SCALE(channels->ch1); + _channels[2] = CHANNEL_SCALE(channels->ch2); + _channels[3] = CHANNEL_SCALE(channels->ch3); + } + +#define CHANNEL_LR_RESCALE(x) (5 * x - 108) +#define CHANNEL_LR_SCALE(x) (int32_t(x) * 2 + 988) +#define CHANNEL_LR_SCALE_LEGACY(x) (CHANNEL_LR_RESCALE(x) + 988) + + switch (_frame.type) { + case GHST_UL_RC_CHANS_HS4_5TO8: + case GHST_UL_RC_CHANS_HS4_9TO12: + case GHST_UL_RC_CHANS_HS4_13TO16: { + uint8_t offset = (_frame.type - GHST_UL_RC_CHANS_HS4_5TO8 + 1) * 4; + _channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[0]); + _channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[1]); + _channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[2]); + _channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[3]); + break; + } + case GHST_UL_RC_CHANS_HS4_12_5TO8: + case GHST_UL_RC_CHANS_HS4_12_9TO12: + case GHST_UL_RC_CHANS_HS4_12_13TO16: { + uint8_t offset = (_frame.type - GHST_UL_RC_CHANS_HS4_12_5TO8 + 1) * 4; + _channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[0]); + _channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[1]); + _channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[2]); + _channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[3]); + break; + } + case GHST_UL_RC_CHANS_RSSI: + case GHST_UL_RC_CHANS_12_RSSI: + process_link_stats_frame((uint8_t*)&_frame.payload); + break; + + default: + break; + } + +#if AP_GHST_TELEM_ENABLED + if (AP_GHST_Telem::process_frame(FrameType(_frame.type), (uint8_t*)&_frame.payload)) { + process_telemetry(); + } +#endif + + return true; +} + +// send out telemetry +bool AP_RCProtocol_GHST::process_telemetry(bool check_constraint) +{ + + AP_HAL::UARTDriver *uart = get_current_UART(); + if (!uart) { + return false; + } + + if (!telem_available) { +#if AP_GHST_TELEM_ENABLED && !APM_BUILD_TYPE(APM_BUILD_iofirmware) + if (AP_GHST_Telem::get_telem_data(&_telemetry_frame, is_tx_active())) { + telem_available = true; + } else { + return false; + } +#else + return false; +#endif + } + if (!write_frame(&_telemetry_frame)) { + return false; + } + + // get fresh telem_data in the next call + telem_available = false; + + return true; +} + +// process link statistics to get RSSI +void AP_RCProtocol_GHST::process_link_stats_frame(const void* data) +{ + const LinkStatisticsFrame* link = (const LinkStatisticsFrame*)data; + + uint8_t rssi_dbm; + rssi_dbm = link->rssi_dbm; + _link_status.link_quality = link->link_quality; + if (_use_lq_for_rssi) { + _link_status.rssi = derive_scaled_lq_value(link->link_quality); + } else{ + // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link + if (rssi_dbm < 50) { + _link_status.rssi = 255; + } else if (rssi_dbm > 120) { + _link_status.rssi = 0; + } else { + // this is an approximation recommended by Remo from TBS + _link_status.rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f)); + } + } + + _link_status.rf_mode = link->protocol; +} + +bool AP_RCProtocol_GHST::is_telemetry_supported() const +{ + return _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_NORMAL + || _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_RACE + || _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_LR + || _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_RACE250; +} + +// process a byte provided by a uart +void AP_RCProtocol_GHST::process_byte(uint8_t byte, uint32_t baudrate) +{ + // reject RC data if we have been configured for standalone mode + if (baudrate != GHST_BAUDRATE) { + return; + } + _process_byte(AP_HAL::micros(), byte); +} + +//returns uplink link quality on 0-255 scale +int16_t AP_RCProtocol_GHST::derive_scaled_lq_value(uint8_t uplink_lq) +{ + return int16_t(roundf(constrain_float(uplink_lq*2.5f,0,255))); +} + +namespace AP { + AP_RCProtocol_GHST* ghost() { + return AP_RCProtocol_GHST::get_singleton(); + } +}; + +#endif // AP_RCPROTOCOL_GHST_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.h b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.h new file mode 100644 index 0000000000..e710a70fa9 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.h @@ -0,0 +1,198 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_GHST_ENABLED + +#include "AP_RCProtocol.h" +#include +#include +#include "SoftSerial.h" + +#define GHST_MAX_CHANNELS 16U // Maximum number of channels from GHST datastream +#define GHST_FRAMELEN_MAX 14U // maximum possible framelength +#define GHST_HEADER_LEN 2U // header length +#define GHST_FRAME_PAYLOAD_MAX (GHST_FRAMELEN_MAX - GHST_HEADER_LEN) // maximum size of the frame length field in a packet +#define GHST_BAUDRATE 420000U +#define GHST_TX_TIMEOUT 500000U // the period after which the transmitter is considered disconnected (matches copters failsafe) +#define GHST_RX_TIMEOUT 150000U // the period after which the receiver is considered disconnected (>ping frequency) + +class AP_RCProtocol_GHST : public AP_RCProtocol_Backend { +public: + AP_RCProtocol_GHST(AP_RCProtocol &_frontend); + virtual ~AP_RCProtocol_GHST(); + void process_byte(uint8_t byte, uint32_t baudrate) override; + void update(void) override; + + // is the receiver active, used to detect power loss and baudrate changes + bool is_rx_active() const override { + return AP_HAL::micros() < _last_rx_frame_time_us + GHST_RX_TIMEOUT; + } + + // is the transmitter active, used to adjust telemetry data + bool is_tx_active() const { + // this is the same as the Copter failsafe timeout + return AP_HAL::micros() < _last_tx_frame_time_us + GHST_TX_TIMEOUT; + } + + // get singleton instance + static AP_RCProtocol_GHST* get_singleton() { + return _singleton; + } + + enum FrameType { + GHST_UL_RC_CHANS_HS4_5TO8 = 0x10, // Control packet with 4 primary channels + CH5-8 + GHST_UL_RC_CHANS_HS4_9TO12 = 0x11, // Control packet with 4 primary channels + CH9-12 + GHST_UL_RC_CHANS_HS4_13TO16 = 0x12, // Control packet with 4 primary channels + CH13-16 + GHST_UL_RC_CHANS_RSSI = 0x13, // Control packet with RSSI and LQ data + GHST_UL_RC_VTX_CTRL = 0x14, // Goggle/FC channel changing + // -> 0x1F reserved + GHST_UL_VTX_SETUP = 0x20, // vTx Setup w/o 4 primary channels (GECO Only) + GHST_UL_MSP_REQ = 0x21, // MSP frame, Request + GHST_UL_MSP_WRITE = 0x22, // MSP frame, Write + + GHST_DL_PACK_STAT = 0x23, // Battery Status + GHST_DL_GPS_PRIMARY = 0x25, // Primary GPS Data + GHST_DL_GPS_SECONDARY = 0x26, // Secondary GPS Data + GHST_DL_MAGBARO = 0x27, // Magnetometer, Barometer (and Vario) Data + GHST_DL_MSP_RESP = 0x28, // MSP Response + + GHST_UL_RC_CHANS_HS4_12_5TO8 = 0x30, // Control packet with 4 primary channels + CH5-8 + GHST_UL_RC_CHANS_HS4_12_9TO12 = 0x31, // Control packet with 4 primary channels + CH9-12 + GHST_UL_RC_CHANS_HS4_12_13TO16 = 0x32, // Control packet with 4 primary channels + CH13-16 + GHST_UL_RC_CHANS_12_RSSI = 0x33, // Control packet with RSSI and LQ data + // 0x30 -> 0x3f - raw 12 bit packets + }; + + enum DeviceAddress { + GHST_ADDRESS_FLIGHT_CONTROLLER = 0x82, + GHST_ADDRESS_GOGGLES = 0x83, + GHST_ADDRESS_GHST_RECEIVER = 0x89, + }; + + struct Frame { + uint8_t device_address; + uint8_t length; + uint8_t type; + uint8_t payload[GHST_FRAME_PAYLOAD_MAX - 1]; // type is already accounted for + } PACKED; + + struct Channels12Bit_4Chan { +#if __BYTE_ORDER != __LITTLE_ENDIAN +#error "Only supported on little-endian architectures" +#endif + uint32_t ch0 : 12; + uint32_t ch1 : 12; + uint32_t ch2 : 12; + uint32_t ch3 : 12; + } PACKED; + + struct RadioFrame { +#if __BYTE_ORDER != __LITTLE_ENDIAN +#error "Only supported on little-endian architectures" +#endif + Channels12Bit_4Chan channels; // high-res channels + uint8_t lowres_channels[4]; // low-res channels + } PACKED; + + struct LinkStatisticsFrame { +#if __BYTE_ORDER != __LITTLE_ENDIAN +#error "Only supported on little-endian architectures" +#endif + Channels12Bit_4Chan channels; + uint8_t link_quality; // ( 0 - 100) + uint8_t rssi_dbm; // ( dBm * -1 ) + uint8_t protocol : 5; + uint8_t telemetry : 1; + uint8_t alt_scale : 1; + uint8_t reserved : 1; + int8_t tx_power; + } PACKED; + + enum RFMode { + GHST_RF_MODE_NORMAL = 5, // 55Hz + GHST_RF_MODE_RACE = 6, // 160Hz + GHST_RF_MODE_PURERACE = 7, // 250Hz + GHST_RF_MODE_LR = 8, // 19Hz + GHST_RF_MODE_RACE250 = 10, // 250Hz + GHST_RF_MODE_RACE500 = 11, // 500Hz + GHTS_RF_MODE_SOLID150 = 12, // 150Hz + GHST_RF_MODE_SOLID250 = 13, // 250Hz + RF_MODE_MAX_MODES, + RF_MODE_UNKNOWN, + }; + + struct LinkStatus { + int16_t rssi = -1; + int16_t link_quality = -1; + uint8_t rf_mode; + }; + + bool is_telemetry_supported() const; + + // this will be used by AP_GHST_Telem to access link status data + // from within AP_RCProtocol_GHST thread so no need for cross-thread synch + const volatile LinkStatus& get_link_status() const { + return _link_status; + } + + // return the link rate as defined by the LinkStatistics + uint16_t get_link_rate() const; + + // return the protocol string + const char* get_protocol_string() const; + +private: + struct Frame _frame; + struct Frame _telemetry_frame; + uint8_t _frame_ofs; + uint8_t _frame_crc; + + const uint8_t MAX_CHANNELS = MIN((uint8_t)GHST_MAX_CHANNELS, (uint8_t)MAX_RCIN_CHANNELS); + + static AP_RCProtocol_GHST* _singleton; + + void _process_byte(uint32_t timestamp_us, uint8_t byte); + bool decode_ghost_packet(); + bool process_telemetry(bool check_constraint = true); + void process_link_stats_frame(const void* data); + bool write_frame(Frame* frame); + AP_HAL::UARTDriver* get_current_UART() { return get_available_UART(); } + + uint16_t _channels[GHST_MAX_CHANNELS]; /* buffer for extracted RC channel data as pulsewidth in microseconds */ + + void add_to_buffer(uint8_t index, uint8_t b) { ((uint8_t*)&_frame)[index] = b; } + + uint32_t _last_frame_time_us; + uint32_t _last_tx_frame_time_us; + uint32_t _last_uart_start_time_ms; + uint32_t _last_rx_frame_time_us; + uint32_t _start_frame_time_us; + bool telem_available; + bool _use_lq_for_rssi; + int16_t derive_scaled_lq_value(uint8_t uplink_lq); + + volatile struct LinkStatus _link_status; + + static const uint16_t RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES]; +}; + +namespace AP { + AP_RCProtocol_GHST* ghost(); +}; + +#endif // AP_RCPROTOCOL_GHST_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h index 5619c5373c..d6bef7d61b 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -61,3 +61,7 @@ #ifndef AP_RCPROTOCOL_FASTSBUS_ENABLED #define AP_RCPROTOCOL_FASTSBUS_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && AP_RCPROTOCOL_SBUS_ENABLED #endif + +#ifndef AP_RCPROTOCOL_GHST_ENABLED +#define AP_RCPROTOCOL_GHST_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED +#endif