From 0da6989c8e00ca6b1eabf1e2783fedf064017688 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 13 Dec 2023 11:14:40 +0000 Subject: [PATCH] AP_RCTelemetry: IRC Ghost protocol --- libraries/AP_RCTelemetry/AP_GHST_Telem.cpp | 386 ++++++++++++++++++ libraries/AP_RCTelemetry/AP_GHST_Telem.h | 166 ++++++++ .../AP_RCTelemetry/AP_RCTelemetry_config.h | 4 + 3 files changed, 556 insertions(+) create mode 100644 libraries/AP_RCTelemetry/AP_GHST_Telem.cpp create mode 100644 libraries/AP_RCTelemetry/AP_GHST_Telem.h diff --git a/libraries/AP_RCTelemetry/AP_GHST_Telem.cpp b/libraries/AP_RCTelemetry/AP_GHST_Telem.cpp new file mode 100644 index 0000000000..72e2d2a09e --- /dev/null +++ b/libraries/AP_RCTelemetry/AP_GHST_Telem.cpp @@ -0,0 +1,386 @@ +/* + 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 . +*/ + +#include "AP_RCTelemetry_config.h" + +#if AP_GHST_TELEM_ENABLED + +#include "AP_GHST_Telem.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//#define GHST_DEBUG +#ifdef GHST_DEBUG +# define debug(fmt, args...) hal.console->printf("GHST: " fmt "\n", ##args) +#else +# define debug(fmt, args...) do {} while(0) +#endif + +extern const AP_HAL::HAL& hal; + +AP_GHST_Telem *AP_GHST_Telem::singleton; + +AP_GHST_Telem::AP_GHST_Telem() : AP_RCTelemetry(0) +{ + singleton = this; +} + +AP_GHST_Telem::~AP_GHST_Telem(void) +{ + singleton = nullptr; +} + +bool AP_GHST_Telem::init(void) +{ + // sanity check that we are using a UART for RC input + if (!AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_RCIN, 0)) { + return false; + } + + return AP_RCTelemetry::init(); +} + +/* + setup ready for passthrough telem + */ +void AP_GHST_Telem::setup_wfq_scheduler(void) +{ + // initialize packet weights for the WFQ scheduler + // priority[i] = 1/_scheduler.packet_weight[i] + // rate[i] = LinkRate * ( priority[i] / (sum(priority[1-n])) ) + + // CSRF telemetry rate is 150Hz (4ms) max, so these rates must fit + add_scheduler_entry(50, 120); // Attitude and compass 8Hz + add_scheduler_entry(200, 1000); // VTX parameters 1Hz + add_scheduler_entry(1300, 500); // battery 2Hz + add_scheduler_entry(550, 280); // GPS 3Hz + add_scheduler_entry(550, 280); // GPS2 3Hz +} + +void AP_GHST_Telem::update_custom_telemetry_rates(AP_RCProtocol_GHST::RFMode rf_mode) +{ + if (is_high_speed_telemetry(rf_mode)) { + // standard telemetry for high data rates + set_scheduler_entry(BATTERY, 1000, 1000); // 1Hz + set_scheduler_entry(ATTITUDE, 1000, 1000); // 1Hz + // custom telemetry for high data rates + set_scheduler_entry(GPS, 550, 500); // 2.0Hz + set_scheduler_entry(GPS2, 550, 500); // 2.0Hz + } else { + // standard telemetry for low data rates + set_scheduler_entry(BATTERY, 1000, 2000); // 0.5Hz + set_scheduler_entry(ATTITUDE, 1000, 3000); // 0.33Hz + // GHST custom telemetry for low data rates + set_scheduler_entry(GPS, 550, 1000); // 1.0Hz + set_scheduler_entry(GPS2, 550, 1000); // 1.0Hz + } +} + +bool AP_GHST_Telem::process_rf_mode_changes() +{ + const AP_RCProtocol_GHST::RFMode current_rf_mode = get_rf_mode(); + uint32_t now = AP_HAL::millis(); + + AP_RCProtocol_GHST* ghost = AP::ghost(); + AP_HAL::UARTDriver* uart = nullptr; + if (ghost != nullptr) { + uart = ghost->get_UART(); + } + if (uart == nullptr) { + return true; + } + // not ready yet + if (!uart->is_initialized()) { + return false; + } +#if !defined (STM32H7) + // warn the user if their setup is sub-optimal, H7 does not need DMA on serial port + if (_telem_bootstrap_msg_pending && !uart->is_dma_enabled()) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s: running on non-DMA serial port", get_protocol_string()); + } +#endif + // note if option was set to show LQ in place of RSSI + bool current_lq_as_rssi_active = rc().option_is_enabled(RC_Channels::Option::USE_CRSF_LQ_AS_RSSI); + if(_telem_bootstrap_msg_pending || _noted_lq_as_rssi_active != current_lq_as_rssi_active){ + _noted_lq_as_rssi_active = current_lq_as_rssi_active; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s: RSSI now displays %s", get_protocol_string(), current_lq_as_rssi_active ? " as LQ" : "normally"); + } + _telem_bootstrap_msg_pending = false; + + const bool is_high_speed = is_high_speed_telemetry(current_rf_mode); + if ((now - _telem_last_report_ms > 5000)) { + // report an RF mode change or a change in telemetry rate if we haven't done so in the last 5s + if (!rc().option_is_enabled(RC_Channels::Option::SUPPRESS_CRSF_MESSAGE) && (_rf_mode != current_rf_mode)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s: Link rate %dHz, Telemetry %s", + get_protocol_string(), ghost->get_link_rate(), _enable_telemetry ? "ON" : "OFF"); + } + // tune the scheduler based on telemetry speed high/low transitions + if (_telem_is_high_speed != is_high_speed) { + update_custom_telemetry_rates(current_rf_mode); + } + _telem_is_high_speed = is_high_speed; + _rf_mode = current_rf_mode; + _telem_last_avg_rate = _scheduler.avg_packet_rate; + if (_telem_last_report_ms == 0) { // only want to show bootstrap messages once + _telem_bootstrap_msg_pending = true; + } + _telem_last_report_ms = now; + } + return true; +} + +AP_RCProtocol_GHST::RFMode AP_GHST_Telem::get_rf_mode() const +{ + AP_RCProtocol_GHST* ghost = AP::ghost(); + if (ghost == nullptr) { + return AP_RCProtocol_GHST::RFMode::RF_MODE_UNKNOWN; + } + + return static_cast(ghost->get_link_status().rf_mode); +} + +bool AP_GHST_Telem::is_high_speed_telemetry(const AP_RCProtocol_GHST::RFMode rf_mode) const +{ + return rf_mode == AP_RCProtocol_GHST::RFMode::GHST_RF_MODE_RACE || rf_mode == AP_RCProtocol_GHST::RFMode::GHST_RF_MODE_RACE250; +} + +uint16_t AP_GHST_Telem::get_telemetry_rate() const +{ + return get_avg_packet_rate(); +} + +// WFQ scheduler +bool AP_GHST_Telem::is_packet_ready(uint8_t idx, bool queue_empty) +{ + return _enable_telemetry; +} + +// WFQ scheduler +void AP_GHST_Telem::process_packet(uint8_t idx) +{ + // send packet + switch (idx) { + case ATTITUDE: + calc_attitude(); + break; + case BATTERY: // BATTERY + calc_battery(); + break; + case GPS: // GPS + calc_gps(); + break; + case GPS2: // GPS secondary info + calc_gps2(); + break; + default: + break; + } +} + +// Process a frame from the GHST protocol decoder +bool AP_GHST_Telem::_process_frame(AP_RCProtocol_GHST::FrameType frame_type, void* data) { + switch (frame_type) { + // this means we are connected to an RC receiver and can send telemetry + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_RSSI: { + process_rf_mode_changes(); + _enable_telemetry = AP::ghost()->is_telemetry_supported(); + break; + } + default: + break; + } + return true; +} + +// process any changed settings and schedule for transmission +void AP_GHST_Telem::update() +{ +} + +void AP_GHST_Telem::process_pending_requests() +{ + _pending_request.frame_type = 0; +} + +// prepare battery data +void AP_GHST_Telem::calc_battery() +{ + debug("BATTERY"); + const AP_BattMonitor &_battery = AP::battery(); + + _telem.battery.voltage = htole16(uint16_t(roundf(_battery.voltage(0) * 100.0f))); + + float current; + if (!_battery.current_amps(current, 0)) { + current = 0; + } + _telem.battery.current = htole16(uint16_t(roundf(current * 100.0f))); + + float used_mah; + if (!_battery.consumed_mah(used_mah, 0)) { + used_mah = 0; + } + + _telem.battery.consumed = htole16(uint16_t(roundf(used_mah * 100.0f)));; + _telem.battery.rx_voltage = htole16(uint16_t(roundf(hal.analogin->board_voltage() * 10))); + + _telem_size = sizeof(BatteryFrame); + _telem_type = AP_RCProtocol_GHST::GHST_DL_PACK_STAT; + + _telem_pending = true; +} + +// prepare gps data +void AP_GHST_Telem::calc_gps() +{ + debug("GPS"); + const Location &loc = AP::gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw) + + _telem.gps.latitude = htole32(loc.lat); + _telem.gps.longitude = htole32(loc.lng); + _telem.gps.altitude = htole16(constrain_int16(loc.alt / 100, 0, 5000) + 1000); + + _telem_size = sizeof(AP_GHST_Telem::GPSFrame); + _telem_type = AP_RCProtocol_GHST::GHST_DL_GPS_PRIMARY; + + _telem_pending = true; +} + +void AP_GHST_Telem::calc_gps2() +{ + debug("GPS2"); + _telem.gps2.groundspeed = htole16(roundf(AP::gps().ground_speed() * 100000 / 3600)); + _telem.gps2.gps_heading = htole16(roundf(AP::gps().ground_course() * 100.0f)); + _telem.gps2.satellites = AP::gps().num_sats(); + + AP_AHRS &_ahrs = AP::ahrs(); + WITH_SEMAPHORE(_ahrs.get_semaphore()); + Location loc; + + if (_ahrs.get_location(loc) && _ahrs.home_is_set()) { + const Location &home_loc = _ahrs.get_home(); + _telem.gps2.home_dist = home_loc.get_distance(loc) / 10; // 10m + _telem.gps2.home_heading = loc.get_bearing_to(home_loc) / 10; // deci-degrees + } else { + _telem.gps2.home_dist = 0; + _telem.gps2.home_heading = 0; + } + + AP_GPS::GPS_Status status = AP::gps().status(); + _telem.gps2.flags = status >= AP_GPS::GPS_OK_FIX_2D ? 0x1 : 0; + + _telem_size = sizeof(AP_GHST_Telem::GPSSecondaryFrame); + _telem_type = AP_RCProtocol_GHST::GHST_DL_GPS_SECONDARY; + + _telem_pending = true; +} + +// prepare attitude data +void AP_GHST_Telem::calc_attitude() +{ + debug("MAGBARO"); + AP_AHRS &_ahrs = AP::ahrs(); + WITH_SEMAPHORE(_ahrs.get_semaphore()); + + float heading = AP::compass().calculate_heading(_ahrs.get_rotation_body_to_ned()); + _telem.sensor.compass_heading = htole16(degrees(wrap_PI(heading))); + + float alt = AP::baro().get_altitude(); + _telem.sensor.baro_alt = htole16(roundf(alt)); + _telem.sensor.vario = 0; + _telem.sensor.flags = 3; + _telem_size = sizeof(AP_GHST_Telem::SensorFrame); + _telem_type = AP_RCProtocol_GHST::GHST_DL_MAGBARO; + + _telem_pending = true; +} + +/* + fetch GHST frame data + if is_tx_active is true then this will be a request for telemetry after receiving an RC frame + */ +bool AP_GHST_Telem::_get_telem_data(AP_RCProtocol_GHST::Frame* data, bool is_tx_active) +{ + memset(&_telem, 0, sizeof(TelemetryPayload)); + _is_tx_active = is_tx_active; + + run_wfq_scheduler(); + if (!_telem_pending) { + return false; + } + memcpy(data->payload, &_telem, _telem_size); + data->device_address = AP_RCProtocol_GHST::GHST_ADDRESS_GHST_RECEIVER; + data->length = _telem_size + 2; + data->type = _telem_type; + + _telem_pending = false; + return true; +} + +/* + fetch data for an external transport, such as GHST + */ +bool AP_GHST_Telem::process_frame(AP_RCProtocol_GHST::FrameType frame_type, void* data) +{ + if (!get_singleton()) { + return false; + } + return singleton->_process_frame(frame_type, data); +} + +/* + fetch data for an external transport, such as GHST + */ +bool AP_GHST_Telem::get_telem_data(AP_RCProtocol_GHST::Frame* data, bool is_tx_active) +{ + if (!get_singleton()) { + return false; + } + return singleton->_get_telem_data(data, is_tx_active); +} + +AP_GHST_Telem *AP_GHST_Telem::get_singleton(void) { + if (!singleton && !hal.util->get_soft_armed()) { + // if telem data is requested when we are disarmed and don't + // yet have a AP_GHST_Telem object then try to allocate one + new AP_GHST_Telem(); + // initialize the passthrough scheduler + if (singleton) { + singleton->init(); + } + } + return singleton; +} + +namespace AP { + AP_GHST_Telem *ghost_telem() { + return AP_GHST_Telem::get_singleton(); + } +}; + +#endif // AP_GHST_TELEM_ENABLED diff --git a/libraries/AP_RCTelemetry/AP_GHST_Telem.h b/libraries/AP_RCTelemetry/AP_GHST_Telem.h new file mode 100644 index 0000000000..768470d997 --- /dev/null +++ b/libraries/AP_RCTelemetry/AP_GHST_Telem.h @@ -0,0 +1,166 @@ +/* + 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 . +*/ +#pragma once + +#include "AP_RCTelemetry_config.h" + +#if AP_GHST_TELEM_ENABLED + +#include +#include "AP_RCTelemetry.h" +#include + +class AP_GHST_Telem : public AP_RCTelemetry { +public: + AP_GHST_Telem(); + ~AP_GHST_Telem() override; + + /* Do not allow copies */ + CLASS_NO_COPY(AP_GHST_Telem); + + // init - perform required initialisation + virtual bool init() override; + + static AP_GHST_Telem *get_singleton(void); + + // Broadcast frame definitions courtesy of TBS + struct PACKED GPSFrame { + uint32_t latitude; // ( degree * 1e7 ) + uint32_t longitude; // (degree * 1e7 ) + int16_t altitude; // ( meter ) + }; + + struct PACKED GPSSecondaryFrame { + uint16_t groundspeed; // ( cm/s ) + uint16_t gps_heading; // ( degree * 10 ) + uint8_t satellites; // in use ( counter ) + uint16_t home_dist; // ( m / 10 ) + uint16_t home_heading; // ( degree * 10 ) + uint8_t flags; // GPS_FLAGS_FIX 0x01, GPS_FLAGS_FIX_HOME 0x2 + }; + + struct PACKED BatteryFrame { + uint16_t voltage; // ( mV / 10 ) + uint16_t current; // ( mA / 10 ) + uint16_t consumed; // ( mAh / 10 ) + uint8_t rx_voltage; // ( mV / 100 ) + uint8_t spare[3]; + }; + + struct PACKED VTXFrame { +#if __BYTE_ORDER != __LITTLE_ENDIAN +#error "Only supported on little-endian architectures" +#endif + uint8_t flags; + uint16_t frequency; // frequency in Mhz + uint16_t power; // power in mw, 0 == off + uint8_t band : 4; // A, B, E, AirWave, Race + uint8_t channel : 4; // 1x-8x + uint8_t spare[3]; + }; + + struct PACKED SensorFrame { + uint16_t compass_heading; // ( deg * 10 ) + int16_t baro_alt; // ( m ) + int16_t vario; // ( m/s * 100 ) + uint8_t spare[3]; + uint8_t flags; // MISC_FLAGS_MAGHEAD 0x01, MISC_FLAGS_BAROALT 0x02, MISC_FLAGS_VARIO 0x04 + }; + + union PACKED TelemetryPayload { + GPSFrame gps; + GPSSecondaryFrame gps2; + BatteryFrame battery; + VTXFrame vtx; + SensorFrame sensor; + }; + + // get the protocol string + const char* get_protocol_string() const { return AP::ghost()->get_protocol_string(); } + + // Process a frame from the CRSF protocol decoder + static bool process_frame(AP_RCProtocol_GHST::FrameType frame_type, void* data); + // process any changed settings and schedule for transmission + void update(); + // get next telemetry data for external consumers of SPort data + static bool get_telem_data(AP_RCProtocol_GHST::Frame* frame, bool is_tx_active); + +private: + + enum SensorType { + ATTITUDE, + VTX_PARAMETERS, + BATTERY, + GPS, + GPS2, + NUM_SENSORS + }; + + // passthrough WFQ scheduler + bool is_packet_ready(uint8_t idx, bool queue_empty) override; + void process_packet(uint8_t idx) override; + void setup_custom_telemetry(); + void update_custom_telemetry_rates(const AP_RCProtocol_GHST::RFMode rf_mode); + + void calc_battery(); + void calc_gps(); + void calc_gps2(); + void calc_attitude(); + void process_pending_requests(); + bool process_rf_mode_changes(); + AP_RCProtocol_GHST::RFMode get_rf_mode() const; + uint16_t get_telemetry_rate() const; + bool is_high_speed_telemetry(const AP_RCProtocol_GHST::RFMode rf_mode) const; + + // setup ready for passthrough operation + void setup_wfq_scheduler(void) override; + + // get next telemetry data for external consumers + bool _get_telem_data(AP_RCProtocol_GHST::Frame* data, bool is_tx_active); + bool _process_frame(AP_RCProtocol_GHST::FrameType frame_type, void* data); + + TelemetryPayload _telem; + uint8_t _telem_size; + uint8_t _telem_type; + + AP_RCProtocol_GHST::RFMode _rf_mode; + bool _enable_telemetry; + + // reporting telemetry rate + uint32_t _telem_last_report_ms; + uint16_t _telem_last_avg_rate; + // do we need to report the initial state + bool _telem_bootstrap_msg_pending; + + bool _telem_is_high_speed; + bool _telem_pending; + // used to limit telemetry when in a failsafe condition + bool _is_tx_active; + + struct { + uint8_t destination = 0; + uint8_t frame_type; + } _pending_request; + + bool _noted_lq_as_rssi_active; + + static AP_GHST_Telem *singleton; +}; + +namespace AP { + AP_GHST_Telem *ghost_telem(); +}; + +#endif // AP_GHST_TELEM_ENABLED diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry_config.h b/libraries/AP_RCTelemetry/AP_RCTelemetry_config.h index 8cfb33467d..9dab308187 100644 --- a/libraries/AP_RCTelemetry/AP_RCTelemetry_config.h +++ b/libraries/AP_RCTelemetry/AP_RCTelemetry_config.h @@ -16,3 +16,7 @@ #ifndef HAL_SPEKTRUM_TELEM_ENABLED #define HAL_SPEKTRUM_TELEM_ENABLED 1 #endif + +#ifndef AP_GHST_TELEM_ENABLED +#define AP_GHST_TELEM_ENABLED AP_RCPROTOCOL_GHST_ENABLED +#endif