From 44e5171f2b92f245b0de4f4dc346b58796ea8717 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 4 Apr 2020 17:25:50 +0100 Subject: [PATCH] AP_Frsky_Telem: refactor common code into AP_RCTelemetry refactor naming when using external data AP_Frsky_Telem::init() has to call AP_RCTelemetry::init() and exit. No need to initialize serial ports --- libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 313 +++++--------------- libraries/AP_Frsky_Telem/AP_Frsky_Telem.h | 75 ++--- 2 files changed, 92 insertions(+), 296 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index b8102dabec..5e6a8971c1 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -32,14 +32,13 @@ #include #include #include -#include #include extern const AP_HAL::HAL& hal; AP_Frsky_Telem *AP_Frsky_Telem::singleton; -AP_Frsky_Telem::AP_Frsky_Telem(bool _external_data) : +AP_Frsky_Telem::AP_Frsky_Telem(bool _external_data) : AP_RCTelemetry(TIME_SLOT_MAX), use_external_data(_external_data) { singleton = this; @@ -53,35 +52,22 @@ AP_Frsky_Telem::~AP_Frsky_Telem(void) /* setup ready for passthrough telem */ -void AP_Frsky_Telem::setup_passthrough(void) +void AP_Frsky_Telem::setup_wfq_scheduler(void) { -#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) - // 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); - } -#endif - // initialize packet weights for the WFQ scheduler - // priority[i] = 1/_passthrough.packet_weight[i] + // priority[i] = 1/_scheduler.packet_weight[i] // rate[i] = LinkRate * ( priority[i] / (sum(priority[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 + set_scheduler_entry(TEXT, 35, 28); // 0x5000 status text (dynamic) + set_scheduler_entry(ATTITUDE, 50, 38); // 0x5006 Attitude and range (dynamic) + set_scheduler_entry(GPS_LAT, 550, 280); // 0x800 GPS lat + set_scheduler_entry(GPS_LON, 550, 280); // 0x800 GPS lon + set_scheduler_entry(VEL_YAW, 400, 250); // 0x5005 Vel and Yaw + set_scheduler_entry(AP_STATUS, 700, 500); // 0x5001 AP status + set_scheduler_entry(GPS_STATUS, 700, 500); // 0x5002 GPS status + set_scheduler_entry(HOME, 400, 500); // 0x5004 Home + set_scheduler_entry(BATT_2, 1300, 500); // 0x5008 Battery 2 status + set_scheduler_entry(BATT_1, 1300, 500); // 0x5008 Battery 1 status + set_scheduler_entry(PARAM, 1700, 1000); // 0x5007 parameters } /* @@ -89,6 +75,10 @@ void AP_Frsky_Telem::setup_passthrough(void) */ bool AP_Frsky_Telem::init() { + 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) @@ -98,7 +88,7 @@ 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) - setup_passthrough(); + AP_RCTelemetry::init(); } if (_port != nullptr) { @@ -115,128 +105,86 @@ bool AP_Frsky_Telem::init() return false; } -void AP_Frsky_Telem::update_avg_packet_rate() +void AP_Frsky_Telem::adjust_packet_weight(bool queue_empty) { - uint32_t poll_now = AP_HAL::millis(); - - _passthrough.avg_packet_counter++; - - if (poll_now - _passthrough.last_poll_timer > 1000) { //average in last 1000ms - // initialize - if (_passthrough.avg_packet_rate == 0) _passthrough.avg_packet_rate = _passthrough.avg_packet_counter; - // moving average - _passthrough.avg_packet_rate = (uint8_t)_passthrough.avg_packet_rate * 0.75 + _passthrough.avg_packet_counter * 0.25; - // reset - _passthrough.last_poll_timer = poll_now; - _passthrough.avg_packet_counter = 0; + if (!queue_empty) { + _scheduler.packet_weight[TEXT] = 45; // messages + _scheduler.packet_weight[ATTITUDE] = 80; // attitude + } else { + _scheduler.packet_weight[TEXT] = 5000; // messages + _scheduler.packet_weight[ATTITUDE] = 45; // attitude } } +// WFQ scheduler +bool AP_Frsky_Telem::is_packet_ready(uint8_t idx, bool queue_empty) +{ + bool packet_ready = false; + switch (idx) { + case TEXT: + packet_ready = !queue_empty; + break; + case AP_STATUS: + packet_ready = gcs().vehicle_initialised(); + break; + case BATT_2: + packet_ready = AP::battery().num_instances() > 1; + break; + default: + packet_ready = true; + break; + } + + return packet_ready; +} + /* * WFQ scheduler * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -void AP_Frsky_Telem::passthrough_wfq_adaptive_scheduler(void) +void AP_Frsky_Telem::process_packet(uint8_t idx) { - update_avg_packet_rate(); - - uint32_t now = AP_HAL::millis(); - uint8_t max_delay_idx = 0; - - float max_delay = 0; - float delay = 0; - bool packet_ready = false; - - // build message queue for sensor_status_flags - check_sensor_status_flags(); - // build message queue for ekf_status - check_ekf_status(); - - // dynamic priorities - bool queue_empty; - { - WITH_SEMAPHORE(_statustext.sem); - queue_empty = !_statustext.available && _statustext.queue.empty(); - } - if (!queue_empty) { - _passthrough.packet_weight[0] = 45; // messages - _passthrough.packet_weight[1] = 80; // attitude - } else { - _passthrough.packet_weight[0] = 5000; // messages - _passthrough.packet_weight[1] = 45; // attitude - } - - // search the packet with the longest delay after the scheduled time - for (int i=0;i(_passthrough.packet_weight[i]); - // use >= so with equal delays we choose the packet with lowest priority - // this is ensured by the packets being sorted by desc frequency - // apply the rate limiter - if (delay >= max_delay && ((now - _passthrough.packet_timer[i]) >= _sport_config.packet_min_period[i])) { - switch (i) { - case 0: - packet_ready = !queue_empty; - break; - case 5: - packet_ready = gcs().vehicle_initialised(); - break; - case 8: - packet_ready = AP::battery().num_instances() > 1; - break; - default: - packet_ready = true; - break; - } - - if (packet_ready) { - max_delay = delay; - max_delay_idx = i; - } - } - } - _passthrough.packet_timer[max_delay_idx] = AP_HAL::millis(); // send packet - switch (max_delay_idx) { - case 0: // 0x5000 status text + switch (idx) { + case TEXT: // 0x5000 status text if (get_next_msg_chunk()) { send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID, _msg_chunk.chunk); } break; - case 1: // 0x5006 Attitude and range + case ATTITUDE: // 0x5006 Attitude and range send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+6, calc_attiandrng()); break; - case 2: // 0x800 GPS lat + case GPS_LAT: // 0x800 GPS lat // sample both lat and lon at the same time send_uint32(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude _passthrough.gps_lng_sample = calc_gps_latlng(&_passthrough.send_latitude); // force the scheduler to select GPS lon as packet that's been waiting the most // this guarantees that gps coords are sent at max - // _passthrough.avg_polling_period*number_of_downlink_sensors time separation - _passthrough.packet_timer[3] = _passthrough.packet_timer[2] - 10000; + // _scheduler.avg_polling_period*number_of_downlink_sensors time separation + _scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000; break; - case 3: // 0x800 GPS lon + case GPS_LON: // 0x800 GPS lon send_uint32(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, _passthrough.gps_lng_sample); // gps longitude break; - case 4: // 0x5005 Vel and Yaw + case VEL_YAW: // 0x5005 Vel and Yaw send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+5, calc_velandyaw()); break; - case 5: // 0x5001 AP status + case AP_STATUS: // 0x5001 AP status send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+1, calc_ap_status()); break; - case 6: // 0x5002 GPS Status + case GPS_STATUS: // 0x5002 GPS Status send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+2, calc_gps_status()); break; - case 7: // 0x5004 Home + case HOME: // 0x5004 Home send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+4, calc_home()); break; - case 8: // 0x5008 Battery 2 status + case BATT_2: // 0x5008 Battery 2 status send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+8, calc_batt(1)); break; - case 9: // 0x5003 Battery 1 status + case BATT_1: // 0x5003 Battery 1 status send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+3, calc_batt(0)); break; - case 10: // 0x5007 parameters + case PARAM: // 0x5007 parameters send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param()); break; } @@ -268,7 +216,7 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void) } if (prev_byte == START_STOP_SPORT) { if (_passthrough.new_byte == SENSOR_ID_28) { // byte 0x7E is the header of each poll request - passthrough_wfq_adaptive_scheduler(); + run_wfq_scheduler(); } } } @@ -603,11 +551,11 @@ bool AP_Frsky_Telem::get_next_msg_chunk(void) // on slow links reduce the number of duplicate chunks uint8_t extra_chunks = 2; - if (_passthrough.avg_packet_rate < 20) { + if (_scheduler.avg_packet_rate < 20) { // with 3 or more extra frsky sensors on the bus // send messages only once extra_chunks = 0; - } else if (_passthrough.avg_packet_rate < 30) { + } else if (_scheduler.avg_packet_rate < 30) { // with 1 or 2 extra frsky sensors on the bus // send messages twice extra_chunks = 1; @@ -623,121 +571,6 @@ bool AP_Frsky_Telem::get_next_msg_chunk(void) return true; } -/* - * add message to message cue for transmission through FrSky link - * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) - */ -void AP_Frsky_Telem::queue_message(MAV_SEVERITY severity, const char *text) -{ - mavlink_statustext_t statustext{}; - - statustext.severity = severity; - strncpy(statustext.text, text, sizeof(statustext.text)); - - // The force push will ensure comm links do not block other comm links forever if they fail. - // If we push to a full buffer then we overwrite the oldest entry, effectively removing the - // block but not until the buffer fills up. - WITH_SEMAPHORE(_statustext.sem); - _statustext.queue.push_force(statustext); -} - -/* - * add sensor_status_flags information to message cue, normally passed as sys_status mavlink messages to the GCS, for transmission through FrSky link - * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) - */ -void AP_Frsky_Telem::check_sensor_status_flags(void) -{ - uint32_t now = AP_HAL::millis(); - - const uint32_t _sensor_status_flags = sensor_status_flags(); - - if ((now - check_sensor_status_timer) >= 5000) { // prevent repeating any system_status messages unless 5 seconds have passed - // only one error is reported at a time (in order of preference). Same setup and displayed messages as Mission Planner. - if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_GPS) > 0) { - queue_message(MAV_SEVERITY_CRITICAL, "Bad GPS Health"); - check_sensor_status_timer = now; - } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_GYRO) > 0) { - queue_message(MAV_SEVERITY_CRITICAL, "Bad Gyro Health"); - check_sensor_status_timer = now; - } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_ACCEL) > 0) { - queue_message(MAV_SEVERITY_CRITICAL, "Bad Accel Health"); - check_sensor_status_timer = now; - } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_MAG) > 0) { - queue_message(MAV_SEVERITY_CRITICAL, "Bad Compass Health"); - check_sensor_status_timer = now; - } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE) > 0) { - queue_message(MAV_SEVERITY_CRITICAL, "Bad Baro Health"); - check_sensor_status_timer = now; - } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_LASER_POSITION) > 0) { - queue_message(MAV_SEVERITY_CRITICAL, "Bad LiDAR Health"); - check_sensor_status_timer = now; - } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW) > 0) { - queue_message(MAV_SEVERITY_CRITICAL, "Bad OptFlow Health"); - check_sensor_status_timer = now; - } else if ((_sensor_status_flags & MAV_SYS_STATUS_TERRAIN) > 0) { - queue_message(MAV_SEVERITY_CRITICAL, "Bad or No Terrain Data"); - check_sensor_status_timer = now; - } else if ((_sensor_status_flags & MAV_SYS_STATUS_GEOFENCE) > 0) { - queue_message(MAV_SEVERITY_CRITICAL, "Geofence Breach"); - check_sensor_status_timer = now; - } else if ((_sensor_status_flags & MAV_SYS_STATUS_AHRS) > 0) { - queue_message(MAV_SEVERITY_CRITICAL, "Bad AHRS"); - check_sensor_status_timer = now; - } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_RC_RECEIVER) > 0) { - queue_message(MAV_SEVERITY_CRITICAL, "No RC Receiver"); - check_sensor_status_timer = now; - } else if ((_sensor_status_flags & MAV_SYS_STATUS_LOGGING) > 0) { - queue_message(MAV_SEVERITY_CRITICAL, "Bad Logging"); - check_sensor_status_timer = now; - } - } -} - -/* - * add innovation variance information to message cue, normally passed as ekf_status_report mavlink messages to the GCS, for transmission through FrSky link - * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) - */ -void AP_Frsky_Telem::check_ekf_status(void) -{ - - // get variances - bool get_variance; - float velVar, posVar, hgtVar, tasVar; - Vector3f magVar; - Vector2f offset; - { - AP_AHRS &_ahrs = AP::ahrs(); - WITH_SEMAPHORE(_ahrs.get_semaphore()); - get_variance = _ahrs.get_variances(velVar, posVar, hgtVar, magVar, tasVar, offset); - } - if (get_variance) { - uint32_t now = AP_HAL::millis(); - if ((now - check_ekf_status_timer) >= 10000) { // prevent repeating any ekf_status message unless 10 seconds have passed - // multiple errors can be reported at a time. Same setup as Mission Planner. - if (velVar >= 1) { - queue_message(MAV_SEVERITY_CRITICAL, "Error velocity variance"); - check_ekf_status_timer = now; - } - if (posVar >= 1) { - queue_message(MAV_SEVERITY_CRITICAL, "Error pos horiz variance"); - check_ekf_status_timer = now; - } - if (hgtVar >= 1) { - queue_message(MAV_SEVERITY_CRITICAL, "Error pos vert variance"); - check_ekf_status_timer = now; - } - if (magVar.length() >= 1) { - queue_message(MAV_SEVERITY_CRITICAL, "Error compass variance"); - check_ekf_status_timer = now; - } - if (tasVar >= 1) { - queue_message(MAV_SEVERITY_CRITICAL, "Error terrain alt variance"); - check_ekf_status_timer = now; - } - } - } -} - /* * prepare parameter data * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) @@ -1130,22 +963,12 @@ void AP_Frsky_Telem::calc_gps_position(void) _SPort_data.yaw = (uint16_t)((_ahrs.yaw_sensor / 100) % 360); // heading in degree based on AHRS and not GPS } -uint32_t AP_Frsky_Telem::sensor_status_flags() const -{ - uint32_t present; - uint32_t enabled; - uint32_t health; - gcs().get_sensor_status_flags(present, enabled, health); - - 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) { - passthrough_wfq_adaptive_scheduler(); + run_wfq_scheduler(); if (!external_data.pending) { return false; } @@ -1167,7 +990,7 @@ bool AP_Frsky_Telem::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &d new AP_Frsky_Telem(true); // initialize the passthrough scheduler if (singleton) { - singleton->setup_passthrough(); + singleton->init(); } } if (!singleton) { diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index dbd6265cc7..55d9edc745 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -18,6 +18,7 @@ #include #include #include +#include #define FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY 5 // size of the message buffer queue (max number of messages waiting to be sent) @@ -111,7 +112,7 @@ for FrSky SPort Passthrough // for fair scheduler #define TIME_SLOT_MAX 11 -class AP_Frsky_Telem { +class AP_Frsky_Telem : public AP_RCTelemetry { public: AP_Frsky_Telem(bool external_data=false); @@ -122,16 +123,7 @@ public: AP_Frsky_Telem &operator=(const AP_Frsky_Telem&) = delete; // init - perform required initialisation - bool init(); - - // add statustext message to FrSky lib message queue - void queue_message(MAV_SEVERITY severity, const char *text); - - // update error mask of sensors and subsystems. The mask uses the - // MAV_SYS_STATUS_* values from mavlink. If a bit is set then it - // indicates that the sensor or subsystem is present but not - // functioning correctly - uint32_t sensor_status_flags() const; + virtual bool init() override; static AP_Frsky_Telem *get_singleton(void) { return singleton; @@ -145,17 +137,22 @@ private: AP_SerialManager::SerialProtocol _protocol; // protocol used - detected using SerialManager's SERIAL#_PROTOCOL parameter uint16_t _crc; - uint32_t check_sensor_status_timer; - uint32_t check_ekf_status_timer; uint8_t _paramID; - - struct { - HAL_Semaphore sem; - ObjectBuffer queue{FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY}; - mavlink_statustext_t next; - bool available; - } _statustext; + 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 + }; + struct { int32_t vario_vspd; @@ -177,31 +174,8 @@ private: { bool send_latitude; // sizeof(bool) = 4 ? uint32_t gps_lng_sample; - uint32_t last_poll_timer; - uint32_t avg_packet_counter; - uint32_t packet_timer[TIME_SLOT_MAX]; - uint32_t packet_weight[TIME_SLOT_MAX]; - uint8_t avg_packet_rate; uint8_t new_byte; } _passthrough; - - - struct - { - const uint32_t packet_min_period[TIME_SLOT_MAX] = { - 28, //0x5000 text, 25Hz - 38, //0x5006 attitude 20Hz - 280, //0x800 GPS 3Hz - 280, //0x800 GPS 3Hz - 250, //0x5005 vel&yaw 4Hz - 500, //0x5001 AP status 2Hz - 500, //0x5002 GPS status 2Hz - 500, //0x5004 home 2Hz - 500, //0x5008 batt 2 2Hz - 500, //0x5003 batt 1 2Hz - 1000 //0x5007 parameters 1Hz - }; - } _sport_config; struct { @@ -229,8 +203,12 @@ private: float get_vspeed_ms(void); // passthrough WFQ scheduler - void update_avg_packet_rate(); - void passthrough_wfq_adaptive_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 @@ -247,9 +225,7 @@ private: void send_uint32(uint8_t frame, uint16_t id, uint32_t data); // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format - bool get_next_msg_chunk(void); - void check_sensor_status_flags(void); - void check_ekf_status(void); + 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); @@ -265,9 +241,6 @@ private: float format_gps(float dec); void calc_gps_position(void); - // setup ready for passthrough operation - void setup_passthrough(void); - // 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);