diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp new file mode 100644 index 0000000000..0f1bcc848c --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp @@ -0,0 +1,25 @@ +#include "AP_Frsky_Backend.h" + +extern const AP_HAL::HAL& hal; + +bool AP_Frsky_Backend::init() +{ + // if SPort Passthrough is using external data then it will + // override this to do nothing: + return init_serial_port(); +} + +bool AP_Frsky_Backend::init_serial_port() +{ + if (!hal.scheduler->thread_create( + FUNCTOR_BIND_MEMBER(&AP_Frsky_Backend::loop, void), + "FrSky", + 1024, + AP_HAL::Scheduler::PRIORITY_RCIN, + 1)) { + return false; + } + // we don't want flow control for either protocol + _port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); + return true; +} diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h new file mode 100644 index 0000000000..ff765875f7 --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h @@ -0,0 +1,66 @@ +#pragma once + +#include +#include + +class AP_Frsky_Backend { +public: + + AP_Frsky_Backend(AP_HAL::UARTDriver *port) : + _port(port) { } + + virtual ~AP_Frsky_Backend() {} + + virtual bool init(); + virtual void send() = 0; + + // SPort is at 57600, D overrides this + virtual uint32_t initial_baud() const { return 57600; } + + // get next telemetry data for external consumers of SPort data + virtual bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) { + return false; + } + + virtual void queue_text_message(MAV_SEVERITY severity, const char *text) { } + +protected: + + AP_HAL::UARTDriver *_port; // UART used to send data to FrSky receiver + + virtual bool init_serial_port(); + + // methods related to the nuts-and-bolts of sending data + void send_byte(uint8_t value); + void send_uint16(uint16_t id, uint16_t data); + + void calc_nav_alt(void); + void calc_gps_position(void); + + float get_vspeed_ms(void); + + // methods to convert flight controller data to FrSky D or SPort format + float format_gps(float dec); + + struct + { + int32_t vario_vspd; + char lat_ns, lon_ew; + uint16_t latdddmm; + uint16_t latmmmm; + uint16_t londddmm; + uint16_t lonmmmm; + uint16_t alt_gps_meters; + uint16_t alt_gps_cm; + uint16_t alt_nav_meters; + uint16_t alt_nav_cm; + int16_t speed_in_meter; + uint16_t speed_in_centimeter; + uint16_t yaw; + } _SPort_data; + +private: + + void loop(void); + +}; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_D.h b/libraries/AP_Frsky_Telem/AP_Frsky_D.h new file mode 100644 index 0000000000..944f9fe81a --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_D.h @@ -0,0 +1,23 @@ +#pragma once + +#include "AP_Frsky_Backend.h" + +class AP_Frsky_D : public AP_Frsky_Backend { + +public: + + using AP_Frsky_Backend::AP_Frsky_Backend; + +protected: + + void send() override; + uint32_t initial_baud() const override { return 9600; } + +private: + + struct { + uint32_t last_200ms_frame; + uint32_t last_1000ms_frame; + } _D; + +}; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h new file mode 100644 index 0000000000..c3c4f2b34a --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h @@ -0,0 +1,38 @@ +#pragma once + +#include "AP_Frsky_Backend.h" + +class AP_Frsky_SPort : public AP_Frsky_Backend { + +public: + + using AP_Frsky_Backend::AP_Frsky_Backend; + + void send() override; + +protected: + + void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data); + + struct PACKED + { + bool send_latitude; // sizeof(bool) = 4 ? + uint32_t gps_lng_sample; + uint8_t new_byte; + } _passthrough; + + uint32_t calc_gps_latlng(bool *send_latitude); + +private: + + struct + { + bool sport_status; + bool gps_refresh; + bool vario_refresh; + uint8_t fas_call; + uint8_t gps_call; + uint8_t vario_call; + uint8_t various_call; + } _SPort; +}; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp new file mode 100644 index 0000000000..8b773d6b88 --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -0,0 +1,30 @@ +#include "AP_Frsky_SPort_Passthrough.h" + +bool AP_Frsky_SPort_Passthrough::init() +{ + if (!AP_RCTelemetry::init()) { + return false; + } + return AP_Frsky_SPort::init(); +} + +bool AP_Frsky_SPort_Passthrough::init_serial_port() +{ + if (_use_external_data) { + return true; + } + return AP_Frsky_SPort::init_serial_port(); +} + +void AP_Frsky_SPort_Passthrough::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data) +{ + if (_use_external_data) { + external_data.frame = frame; + external_data.appid = appid; + external_data.data = data; + external_data.pending = true; + return; + } + + return AP_Frsky_SPort::send_sport_frame(frame, appid, data); +} diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h new file mode 100644 index 0000000000..bb3dfda3f7 --- /dev/null +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h @@ -0,0 +1,96 @@ +#pragma once + +#include "AP_Frsky_SPort.h" +#include + +// for fair scheduler +#define TIME_SLOT_MAX 11 + +class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry { +public: + + AP_Frsky_SPort_Passthrough(AP_HAL::UARTDriver *port, bool use_external_data) : + AP_Frsky_SPort(port), + AP_RCTelemetry(TIME_SLOT_MAX), + _use_external_data(use_external_data) + { } + + bool init() override; + bool init_serial_port() override; + + // passthrough WFQ scheduler + bool is_packet_ready(uint8_t idx, bool queue_empty) override; + void process_packet(uint8_t idx) override; + void adjust_packet_weight(bool queue_empty) override; + // setup ready for passthrough operation + void setup_wfq_scheduler(void) override; + + bool get_next_msg_chunk(void) override; + + bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) override; + + void queue_text_message(MAV_SEVERITY severity, const char *text) override { + AP_RCTelemetry::queue_message(severity, text); + } + + +protected: + + void send() override; + +private: + + enum PassthroughParam : uint8_t { + FRAME_TYPE = 1, + BATT_FS_VOLTAGE = 2, + BATT_FS_CAPACITY = 3, + BATT_CAPACITY_1 = 4, + BATT_CAPACITY_2 = 5 + }; + + enum PassthroughPacketType : uint8_t { + TEXT = 0, // 0x5000 status text (dynamic) + ATTITUDE = 1, // 0x5006 Attitude and range (dynamic) + GPS_LAT = 2, // 0x800 GPS lat + GPS_LON = 3, // 0x800 GPS lon + VEL_YAW = 4, // 0x5005 Vel and Yaw + AP_STATUS = 5, // 0x5001 AP status + GPS_STATUS = 6, // 0x5002 GPS status + HOME = 7, // 0x5004 Home + BATT_2 = 8, // 0x5008 Battery 2 status + BATT_1 = 9, // 0x5008 Battery 1 status + PARAM = 10 // 0x5007 parameters + }; + + // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format + uint32_t calc_param(void); + uint32_t calc_batt(uint8_t instance); + uint32_t calc_ap_status(void); + uint32_t calc_home(void); + uint32_t calc_velandyaw(void); + uint32_t calc_attiandrng(void); + + // use_external_data is set when this library will + // be providing data to another transport, such as FPort + bool _use_external_data; + struct { + uint8_t frame; + uint16_t appid; + uint32_t data; + bool pending; + } external_data; + + struct + { + uint32_t chunk; // a "chunk" (four characters/bytes) at a time of the queued message to be sent + uint8_t repeats; // send each message "chunk" 3 times to make sure the entire messsage gets through without getting cut + uint8_t char_index; // index of which character to get in the message + } _msg_chunk; + + void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data); + + uint8_t _paramID; + + uint32_t calc_gps_status(void); + uint16_t prep_number(int32_t number, uint8_t digits, uint8_t power); +}; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index 905833711c..1336b0650b 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -34,12 +34,15 @@ #include #include +#include "AP_Frsky_D.h" +#include "AP_Frsky_SPort.h" +#include "AP_Frsky_SPort_Passthrough.h" + extern const AP_HAL::HAL& hal; AP_Frsky_Telem *AP_Frsky_Telem::singleton; -AP_Frsky_Telem::AP_Frsky_Telem(bool _external_data) : AP_RCTelemetry(TIME_SLOT_MAX), - use_external_data(_external_data) +AP_Frsky_Telem::AP_Frsky_Telem() { singleton = this; } @@ -52,7 +55,7 @@ AP_Frsky_Telem::~AP_Frsky_Telem(void) /* setup ready for passthrough telem */ -void AP_Frsky_Telem::setup_wfq_scheduler(void) +void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void) { // initialize packet weights for the WFQ scheduler // priority[i] = 1/_scheduler.packet_weight[i] @@ -73,39 +76,34 @@ void AP_Frsky_Telem::setup_wfq_scheduler(void) /* * init - perform required initialisation */ -bool AP_Frsky_Telem::init() +bool AP_Frsky_Telem::init(bool use_external_data) { - if (use_external_data) { - return AP_RCTelemetry::init(); - } - const AP_SerialManager &serial_manager = AP::serialmanager(); // check for protocol configured for a serial port - only the first serial port with one of these protocols will then run (cannot have FrSky on multiple serial ports) - if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_D, 0))) { - _protocol = AP_SerialManager::SerialProtocol_FrSky_D; // FrSky D protocol (D-receivers) - } else if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort, 0))) { - _protocol = AP_SerialManager::SerialProtocol_FrSky_SPort; // FrSky SPort protocol (X-receivers) - } else if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough, 0))) { - _protocol = AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough; // FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers) - AP_RCTelemetry::init(); - } - - if (_port != nullptr) { - if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Frsky_Telem::loop, void), - "FrSky", - 1024, AP_HAL::Scheduler::PRIORITY_RCIN, 1)) { - return false; - } - // we don't want flow control for either protocol - _port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); - return true; + AP_HAL::UARTDriver *port; + if ((port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_D, 0))) { + _backend = new AP_Frsky_D(port); + } else if ((port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort, 0))) { + _backend = new AP_Frsky_SPort(port); + } else if (use_external_data || (port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough, 0))) { + _backend = new AP_Frsky_SPort_Passthrough(port, use_external_data); + } + + if (_backend == nullptr) { + return false; + } + + if (!_backend->init()) { + delete _backend; + _backend = nullptr; + return false; } - return false; + return true; } -void AP_Frsky_Telem::adjust_packet_weight(bool queue_empty) +void AP_Frsky_SPort_Passthrough::adjust_packet_weight(bool queue_empty) { if (!queue_empty) { _scheduler.packet_weight[TEXT] = 45; // messages @@ -117,7 +115,7 @@ void AP_Frsky_Telem::adjust_packet_weight(bool queue_empty) } // WFQ scheduler -bool AP_Frsky_Telem::is_packet_ready(uint8_t idx, bool queue_empty) +bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty) { bool packet_ready = false; switch (idx) { @@ -142,7 +140,7 @@ bool AP_Frsky_Telem::is_packet_ready(uint8_t idx, bool queue_empty) * WFQ scheduler * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -void AP_Frsky_Telem::process_packet(uint8_t idx) +void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx) { // send packet switch (idx) { @@ -194,7 +192,7 @@ void AP_Frsky_Telem::process_packet(uint8_t idx) * send telemetry data * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -void AP_Frsky_Telem::send_SPort_Passthrough(void) +void AP_Frsky_SPort_Passthrough::send(void) { int16_t numc; numc = _port->available(); @@ -225,7 +223,7 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void) * send telemetry data * for FrSky SPort protocol (X-receivers) */ -void AP_Frsky_Telem::send_SPort(void) +void AP_Frsky_SPort::send(void) { int16_t numc; numc = _port->available(); @@ -356,7 +354,7 @@ void AP_Frsky_Telem::send_SPort(void) * a second frame (frame2) is sent every second (1000ms) with gps position data, and ahrs.yaw_sensor heading (instead of GPS heading) * for FrSky D protocol (D-receivers) */ -void AP_Frsky_Telem::send_D(void) +void AP_Frsky_D::send(void) { const AP_BattMonitor &_battery = AP::battery(); uint32_t now = AP_HAL::millis(); @@ -400,32 +398,21 @@ void AP_Frsky_Telem::send_D(void) /* thread to loop handling bytes */ -void AP_Frsky_Telem::loop(void) +void AP_Frsky_Backend::loop(void) { // initialise uart (this must be called from within tick b/c the UART begin must be called from the same thread as it is used from) - if (_protocol == AP_SerialManager::SerialProtocol_FrSky_D) { // FrSky D protocol (D-receivers) - _port->begin(AP_SERIALMANAGER_FRSKY_D_BAUD, AP_SERIALMANAGER_FRSKY_BUFSIZE_RX, AP_SERIALMANAGER_FRSKY_BUFSIZE_TX); - } else { // FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers) - _port->begin(AP_SERIALMANAGER_FRSKY_SPORT_BAUD, AP_SERIALMANAGER_FRSKY_BUFSIZE_RX, AP_SERIALMANAGER_FRSKY_BUFSIZE_TX); - } - _port->set_unbuffered_writes(true); + _port->begin(initial_baud(), 0, 0); while (true) { hal.scheduler->delay(1); - if (_protocol == AP_SerialManager::SerialProtocol_FrSky_D) { // FrSky D protocol (D-receivers) - send_D(); - } else if (_protocol == AP_SerialManager::SerialProtocol_FrSky_SPort) { // FrSky SPort protocol (X-receivers) - send_SPort(); - } else if (_protocol == AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough) { // FrSky SPort Passthrough (OpenTX) protocol (X-receivers) - send_SPort_Passthrough(); - } + send(); } } /* send 1 byte and do byte stuffing */ -void AP_Frsky_Telem::send_byte(uint8_t byte) +void AP_Frsky_Backend::send_byte(uint8_t byte) { if (byte == START_STOP_D) { _port->write(0x5D); @@ -441,16 +428,8 @@ void AP_Frsky_Telem::send_byte(uint8_t byte) /* * send an 8 bytes SPort frame of FrSky data - for FrSky SPort protocol (X-receivers) */ -void AP_Frsky_Telem::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data) +void AP_Frsky_SPort::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data) { - if (use_external_data) { - external_data.frame = frame; - external_data.appid = appid; - external_data.data = data; - external_data.pending = true; - return; - } - uint8_t buf[8]; buf[0] = frame; @@ -503,7 +482,7 @@ void AP_Frsky_Telem::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t d /* * send one uint16 frame of FrSky data - for FrSky D protocol (D-receivers) */ -void AP_Frsky_Telem::send_uint16(uint16_t id, uint16_t data) +void AP_Frsky_Backend::send_uint16(uint16_t id, uint16_t data) { _port->write(START_STOP_D); // send a 0x5E start byte uint8_t *bytes = (uint8_t*)&id; @@ -517,7 +496,7 @@ void AP_Frsky_Telem::send_uint16(uint16_t id, uint16_t data) * grabs one "chunk" (4 bytes) of the queued message to be transmitted * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -bool AP_Frsky_Telem::get_next_msg_chunk(void) +bool AP_Frsky_SPort_Passthrough::get_next_msg_chunk(void) { if (!_statustext.available) { WITH_SEMAPHORE(_statustext.sem); @@ -578,7 +557,7 @@ bool AP_Frsky_Telem::get_next_msg_chunk(void) * prepare parameter data * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -uint32_t AP_Frsky_Telem::calc_param(void) +uint32_t AP_Frsky_SPort_Passthrough::calc_param(void) { const AP_BattMonitor &_battery = AP::battery(); @@ -615,7 +594,7 @@ uint32_t AP_Frsky_Telem::calc_param(void) * prepare gps latitude/longitude data * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -uint32_t AP_Frsky_Telem::calc_gps_latlng(bool *send_latitude) +uint32_t AP_Frsky_SPort::calc_gps_latlng(bool *send_latitude) { uint32_t latlng; const Location &loc = AP::gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw) @@ -643,7 +622,7 @@ uint32_t AP_Frsky_Telem::calc_gps_latlng(bool *send_latitude) * prepare gps status data * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -uint32_t AP_Frsky_Telem::calc_gps_status(void) +uint32_t AP_Frsky_SPort_Passthrough::calc_gps_status(void) { const AP_GPS &gps = AP::gps(); @@ -667,7 +646,7 @@ uint32_t AP_Frsky_Telem::calc_gps_status(void) * prepare battery data * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -uint32_t AP_Frsky_Telem::calc_batt(uint8_t instance) +uint32_t AP_Frsky_SPort_Passthrough::calc_batt(uint8_t instance) { const AP_BattMonitor &_battery = AP::battery(); @@ -693,7 +672,7 @@ uint32_t AP_Frsky_Telem::calc_batt(uint8_t instance) * prepare various autopilot status data * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -uint32_t AP_Frsky_Telem::calc_ap_status(void) +uint32_t AP_Frsky_SPort_Passthrough::calc_ap_status(void) { uint32_t ap_status; @@ -724,7 +703,7 @@ uint32_t AP_Frsky_Telem::calc_ap_status(void) * prepare home position related data * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -uint32_t AP_Frsky_Telem::calc_home(void) +uint32_t AP_Frsky_SPort_Passthrough::calc_home(void) { uint32_t home = 0; Location loc; @@ -763,7 +742,7 @@ uint32_t AP_Frsky_Telem::calc_home(void) * prepare velocity and yaw data * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -uint32_t AP_Frsky_Telem::calc_velandyaw(void) +uint32_t AP_Frsky_SPort_Passthrough::calc_velandyaw(void) { float vspd = get_vspeed_ms(); // vertical velocity in dm/s @@ -786,7 +765,7 @@ uint32_t AP_Frsky_Telem::calc_velandyaw(void) * prepare attitude (roll, pitch) and range data * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -uint32_t AP_Frsky_Telem::calc_attiandrng(void) +uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void) { const RangeFinder *_rng = RangeFinder::get_singleton(); @@ -805,7 +784,7 @@ uint32_t AP_Frsky_Telem::calc_attiandrng(void) * prepare value for transmission through FrSky link * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -uint16_t AP_Frsky_Telem::prep_number(int32_t number, uint8_t digits, uint8_t power) +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); @@ -870,7 +849,7 @@ uint16_t AP_Frsky_Telem::prep_number(int32_t number, uint8_t digits, uint8_t pow * get vertical speed from ahrs, if not available fall back to baro climbrate, units is m/s * for FrSky D and SPort protocols */ -float AP_Frsky_Telem::get_vspeed_ms(void) +float AP_Frsky_Backend::get_vspeed_ms(void) { {// release semaphore as soon as possible @@ -891,7 +870,7 @@ float AP_Frsky_Telem::get_vspeed_ms(void) * prepare altitude between vehicle and home location data * for FrSky D and SPort protocols */ -void AP_Frsky_Telem::calc_nav_alt(void) +void AP_Frsky_Backend::calc_nav_alt(void) { _SPort_data.vario_vspd = (int32_t)(get_vspeed_ms()*100); //convert to cm/s @@ -916,7 +895,7 @@ void AP_Frsky_Telem::calc_nav_alt(void) * format the decimal latitude/longitude to the required degrees/minutes * for FrSky D and SPort protocols */ -float AP_Frsky_Telem::format_gps(float dec) +float AP_Frsky_Backend::format_gps(float dec) { uint8_t dm_deg = (uint8_t) dec; return (dm_deg * 100.0f) + (dec - dm_deg) * 60; @@ -926,7 +905,7 @@ float AP_Frsky_Telem::format_gps(float dec) * prepare gps data * for FrSky D and SPort protocols */ -void AP_Frsky_Telem::calc_gps_position(void) +void AP_Frsky_Backend::calc_gps_position(void) { float lat; float lon; @@ -971,7 +950,7 @@ void AP_Frsky_Telem::calc_gps_position(void) /* fetch Sport data for an external transport, such as FPort */ -bool AP_Frsky_Telem::_get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) +bool AP_Frsky_SPort_Passthrough::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) { run_wfq_scheduler(); if (!external_data.pending) { @@ -984,6 +963,14 @@ bool AP_Frsky_Telem::_get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t & return true; } +bool AP_Frsky_Telem::_get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) +{ + if (_backend == nullptr) { + return false; + } + return _backend->get_telem_data(frame, appid, data); +} + /* fetch Sport data for an external transport, such as FPort */ @@ -992,10 +979,10 @@ bool AP_Frsky_Telem::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &d if (!singleton && !hal.util->get_soft_armed()) { // if telem data is requested when we are disarmed and don't // yet have a AP_Frsky_Telem object then try to allocate one - new AP_Frsky_Telem(true); + new AP_Frsky_Telem(); // initialize the passthrough scheduler if (singleton) { - singleton->init(); + singleton->init(true); } } if (!singleton) { diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index 0f7070956d..9d65cef868 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -18,7 +18,8 @@ #include #include #include -#include + +#include "AP_Frsky_Backend.h" #define FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY 5 // size of the message buffer queue (max number of messages waiting to be sent) @@ -111,12 +112,10 @@ for FrSky SPort Passthrough #define ATTIANDRNG_PITCH_LIMIT 0x3FF #define ATTIANDRNG_PITCH_OFFSET 11 #define ATTIANDRNG_RNGFND_OFFSET 21 -// for fair scheduler -#define TIME_SLOT_MAX 11 -class AP_Frsky_Telem : public AP_RCTelemetry { +class AP_Frsky_Telem { public: - AP_Frsky_Telem(bool external_data=false); + AP_Frsky_Telem(); ~AP_Frsky_Telem(); @@ -125,7 +124,7 @@ public: AP_Frsky_Telem &operator=(const AP_Frsky_Telem&) = delete; // init - perform required initialisation - virtual bool init() override; + bool init(bool use_external_data=false); static AP_Frsky_Telem *get_singleton(void) { return singleton; @@ -134,135 +133,22 @@ public: // get next telemetry data for external consumers of SPort data static bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data); -private: - AP_HAL::UARTDriver *_port; // UART used to send data to FrSky receiver - AP_SerialManager::SerialProtocol _protocol; // protocol used - detected using SerialManager's SERIAL#_PROTOCOL parameter - uint16_t _crc; - - uint8_t _paramID; - - enum PassthroughPacketType : uint8_t { - TEXT = 0, // 0x5000 status text (dynamic) - ATTITUDE = 1, // 0x5006 Attitude and range (dynamic) - GPS_LAT = 2, // 0x800 GPS lat - GPS_LON = 3, // 0x800 GPS lon - VEL_YAW = 4, // 0x5005 Vel and Yaw - AP_STATUS = 5, // 0x5001 AP status - GPS_STATUS = 6, // 0x5002 GPS status - HOME = 7, // 0x5004 Home - BATT_2 = 8, // 0x5008 Battery 2 status - BATT_1 = 9, // 0x5008 Battery 1 status - PARAM = 10 // 0x5007 parameters - }; - - enum PassthroughParam : uint8_t { - FRAME_TYPE = 1, - BATT_FS_VOLTAGE = 2, - BATT_FS_CAPACITY = 3, - BATT_CAPACITY_1 = 4, - BATT_CAPACITY_2 = 5 - }; - - struct - { - int32_t vario_vspd; - char lat_ns, lon_ew; - uint16_t latdddmm; - uint16_t latmmmm; - uint16_t londddmm; - uint16_t lonmmmm; - uint16_t alt_gps_meters; - uint16_t alt_gps_cm; - uint16_t alt_nav_meters; - uint16_t alt_nav_cm; - int16_t speed_in_meter; - uint16_t speed_in_centimeter; - uint16_t yaw; - } _SPort_data; - - struct PACKED - { - bool send_latitude; // sizeof(bool) = 4 ? - uint32_t gps_lng_sample; - uint8_t new_byte; - } _passthrough; - - struct - { - bool sport_status; - bool gps_refresh; - bool vario_refresh; - uint8_t fas_call; - uint8_t gps_call; - uint8_t vario_call; - uint8_t various_call; - } _SPort; - - struct - { - uint32_t last_200ms_frame; - uint32_t last_1000ms_frame; - } _D; - - struct - { - uint32_t chunk; // a "chunk" (four characters/bytes) at a time of the queued message to be sent - uint8_t repeats; // send each message "chunk" 3 times to make sure the entire messsage gets through without getting cut - uint8_t char_index; // index of which character to get in the message - } _msg_chunk; - - float get_vspeed_ms(void); - // passthrough WFQ scheduler - bool is_packet_ready(uint8_t idx, bool queue_empty) override; - void process_packet(uint8_t idx) override; - void adjust_packet_weight(bool queue_empty) override; - // setup ready for passthrough operation - void setup_wfq_scheduler(void) override; - - // main transmission function when protocol is FrSky SPort Passthrough (OpenTX) - void send_SPort_Passthrough(void); - // main transmission function when protocol is FrSky SPort - void send_SPort(void); - // main transmission function when protocol is FrSky D - void send_D(void); - // tick - main call to send updates to transmitter (called by scheduler at 1kHz) - void loop(void); - // methods related to the nuts-and-bolts of sending data - void send_byte(uint8_t value); - void send_uint16(uint16_t id, uint16_t data); - void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data); + void queue_message(MAV_SEVERITY severity, const char *text) { + if (_backend == nullptr) { + return; + } + return _backend->queue_text_message(severity, text); + } - // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format - bool get_next_msg_chunk(void) override; - uint32_t calc_param(void); - uint32_t calc_gps_latlng(bool *send_latitude); - uint32_t calc_gps_status(void); - uint32_t calc_batt(uint8_t instance); - uint32_t calc_ap_status(void); - uint32_t calc_home(void); - uint32_t calc_velandyaw(void); - uint32_t calc_attiandrng(void); - uint16_t prep_number(int32_t number, uint8_t digits, uint8_t power); +private: - // methods to convert flight controller data to FrSky D or SPort format - void calc_nav_alt(void); - float format_gps(float dec); - void calc_gps_position(void); + AP_Frsky_Backend *_backend; // 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); - + static AP_Frsky_Telem *singleton; - // use_external_data is set when this library will - // be providing data to another transport, such as FPort - bool use_external_data; - struct { - uint8_t frame; - uint16_t appid; - uint32_t data; - bool pending; - } external_data; }; namespace AP {