diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index dd969bd8ea..f9b18179c9 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -37,9 +37,49 @@ extern const AP_HAL::HAL& hal; +AP_Frsky_Telem *AP_Frsky_Telem::singleton; + AP_Frsky_Telem::AP_Frsky_Telem(void) : _statustext_queue(FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY) { + singleton = this; +} + +AP_Frsky_Telem::~AP_Frsky_Telem(void) +{ + singleton = nullptr; +} + +/* + setup ready for passthrough telem + */ +void AP_Frsky_Telem::setup_passthrough(void) +{ + // make frsky_telemetry available to GCS_MAVLINK (used to queue statustext messages from GCS_MAVLINK) + // add firmware and frame info to message queue + const char* _frame_string = gcs().frame_string(); + if (_frame_string == nullptr) { + queue_message(MAV_SEVERITY_INFO, AP::fwversion().fw_string); + } else { + char firmware_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + snprintf(firmware_buf, sizeof(firmware_buf), "%s %s", AP::fwversion().fw_string, _frame_string); + queue_message(MAV_SEVERITY_INFO, firmware_buf); + } + + // initialize packet weights for the WFQ scheduler + // weight[i] = 1/_passthrough.packet_period[i] + // rate[i] = LinkRate * ( weight[i] / (sum(weight[1-n])) ) + _passthrough.packet_weight[0] = 35; // 0x5000 status text (dynamic) + _passthrough.packet_weight[1] = 50; // 0x5006 Attitude and range (dynamic) + _passthrough.packet_weight[2] = 550; // 0x800 GPS lat (600 with 1 sensor) + _passthrough.packet_weight[3] = 550; // 0x800 GPS lon (600 with 1 sensor) + _passthrough.packet_weight[4] = 400; // 0x5005 Vel and Yaw + _passthrough.packet_weight[5] = 700; // 0x5001 AP status + _passthrough.packet_weight[6] = 700; // 0x5002 GPS Status + _passthrough.packet_weight[7] = 400; // 0x5004 Home + _passthrough.packet_weight[8] = 1300; // 0x5008 Battery 2 status + _passthrough.packet_weight[9] = 1300; // 0x5003 Battery 1 status + _passthrough.packet_weight[10] = 1700; // 0x5007 parameters } /* @@ -56,33 +96,20 @@ bool AP_Frsky_Telem::init() _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) - // make frsky_telemetry available to GCS_MAVLINK (used to queue statustext messages from GCS_MAVLINK) - // add firmware and frame info to message queue - const char* _frame_string = gcs().frame_string(); - if (_frame_string == nullptr) { - queue_message(MAV_SEVERITY_INFO, AP::fwversion().fw_string); - } else { - char firmware_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; - snprintf(firmware_buf, sizeof(firmware_buf), "%s %s", AP::fwversion().fw_string, _frame_string); - queue_message(MAV_SEVERITY_INFO, firmware_buf); - } - - // initialize packet weights for the WFQ scheduler - // weight[i] = 1/_passthrough.packet_period[i] - // rate[i] = LinkRate * ( weight[i] / (sum(weight[1-n])) ) - _passthrough.packet_weight[0] = 35; // 0x5000 status text (dynamic) - _passthrough.packet_weight[1] = 50; // 0x5006 Attitude and range (dynamic) - _passthrough.packet_weight[2] = 550; // 0x800 GPS lat (600 with 1 sensor) - _passthrough.packet_weight[3] = 550; // 0x800 GPS lon (600 with 1 sensor) - _passthrough.packet_weight[4] = 400; // 0x5005 Vel and Yaw - _passthrough.packet_weight[5] = 700; // 0x5001 AP status - _passthrough.packet_weight[6] = 700; // 0x5002 GPS Status - _passthrough.packet_weight[7] = 400; // 0x5004 Home - _passthrough.packet_weight[8] = 1300; // 0x5008 Battery 2 status - _passthrough.packet_weight[9] = 1300; // 0x5003 Battery 1 status - _passthrough.packet_weight[10] = 1700; // 0x5007 parameters + setup_passthrough(); + } else if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_RCIN, 0))) { + _protocol = AP_SerialManager::SerialProtocol_RCIN; // FrSky SPort over FPort + setup_passthrough(); } - + + if (_protocol == AP_SerialManager::SerialProtocol_RCIN) { + use_external_data = true; + setup_passthrough(); + // we don't setup the thread, as output is driven by the AP_RCProtocol library + // calling get_telem_data(); + return true; + } + if (_port != nullptr) { if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Frsky_Telem::loop, void), "FrSky", @@ -499,6 +526,13 @@ void AP_Frsky_Telem::send_byte(uint8_t byte) */ void AP_Frsky_Telem::send_uint32(uint8_t frame, uint16_t id, uint32_t data) { + if (use_external_data) { + external_data.frame = frame; + external_data.appid = id; + external_data.data = data; + external_data.pending = true; + return; + } send_byte(frame); // frame type uint8_t *bytes = (uint8_t*)&id; send_byte(bytes[0]); // LSB @@ -1063,3 +1097,29 @@ uint32_t AP_Frsky_Telem::sensor_status_flags() const return ~health & enabled & present; } + +/* + 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) +{ + if (!use_external_data) { + // not initialised for external data + return false; + } + passthrough_wfq_adaptive_scheduler(0); + if (!external_data.pending) { + return false; + } + frame = external_data.frame; + appid = external_data.appid; + data = external_data.data; + external_data.pending = false; + return true; +} + +namespace AP { + AP_Frsky_Telem *frsky_telem() { + return AP_Frsky_Telem::get_singleton(); + } +}; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index 9930a7016a..e1cbb60855 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -114,6 +114,8 @@ class AP_Frsky_Telem { public: AP_Frsky_Telem(); + ~AP_Frsky_Telem(); + /* Do not allow copies */ AP_Frsky_Telem(const AP_Frsky_Telem &other) = delete; AP_Frsky_Telem &operator=(const AP_Frsky_Telem&) = delete; @@ -130,6 +132,13 @@ public: // functioning correctly uint32_t sensor_status_flags() const; + static AP_Frsky_Telem *get_singleton(void) { + return singleton; + } + + // get next telemetry data for external consumers of SPort data + 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 @@ -244,4 +253,23 @@ private: void calc_nav_alt(void); float format_gps(float dec); void calc_gps_position(void); + + // setup ready for passthrough operation + void setup_passthrough(void); + + 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 { + AP_Frsky_Telem *frsky_telem(); };