diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index dae2cee9b8..dd969bd8ea 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -1,6 +1,8 @@ /* - Inspired by work done here https://github.com/PX4/Firmware/tree/master/src/drivers/frsky_telemetry from Stefan Rado + Inspired by work done here + https://github.com/PX4/Firmware/tree/master/src/drivers/frsky_telemetry from Stefan Rado + https://github.com/opentx/opentx/tree/2.3/radio/src/telemetry from the OpenTX team 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 @@ -29,7 +31,9 @@ #include #include #include +#include #include +#include extern const AP_HAL::HAL& hal; @@ -62,6 +66,21 @@ bool AP_Frsky_Telem::init() 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 } if (_port != nullptr) { @@ -78,6 +97,127 @@ bool AP_Frsky_Telem::init() return false; } +void AP_Frsky_Telem::update_avg_packet_rate() +{ + 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; + } +} + +/* + * WFQ scheduler + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +void AP_Frsky_Telem::passthrough_wfq_adaptive_scheduler(uint8_t prev_byte) +{ + uint32_t now = AP_HAL::millis(); + uint8_t max_delay_idx = TIME_SLOT_MAX; + + 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 + if (!_statustext_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 = !_statustext_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 TIME_SLOT_MAX: // nothing to send + break; + case 0: // 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 + send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+6, calc_attiandrng()); + break; + case 2: // 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; + break; + case 3: // 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 + send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+5, calc_velandyaw()); + break; + case 5: // 0x5001 AP status + send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+1, calc_ap_status()); + break; + case 6: // 0x5002 GPS Status + send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+2, calc_gps_status()); + break; + case 7: // 0x5004 Home + send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+4, calc_home()); + break; + case 8: // 0x5008 Battery 2 status + send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+8, calc_batt(1)); + break; + case 9: // 0x5003 Battery 1 status + send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+3, calc_batt(0)); + break; + case 10: // 0x5007 parameters + send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param()); + break; + } +} /* * send telemetry data @@ -97,83 +237,16 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void) if (_port->txspace() < 19) { return; } - // keep only the last two bytes of the data found in the serial buffer, as we shouldn't respond to old poll requests uint8_t prev_byte = 0; for (int16_t i = 0; i < numc; i++) { prev_byte = _passthrough.new_byte; _passthrough.new_byte = _port->read(); } - - if ((prev_byte == START_STOP_SPORT) && (_passthrough.new_byte == SENSOR_ID_28)) { // byte 0x7E is the header of each poll request - if (_passthrough.send_chunk) { // skip other data and send a message chunk during this iteration - _passthrough.send_chunk = false; - if (get_next_msg_chunk()) { - send_uint32(DIY_FIRST_ID, _msg_chunk.chunk); - } - } else { - // build message queue for sensor_status_flags - check_sensor_status_flags(); - // build message queue for ekf_status - check_ekf_status(); - // if there are pending messages in the queue, send them during next iteration - if (!_statustext_queue.empty()) { - _passthrough.send_chunk = true; - } - if (_passthrough.send_attiandrng) { // skip other data, send attitude (roll, pitch) and range only this iteration - _passthrough.send_attiandrng = false; // next iteration, check if we should send something other - } else { // send other sensor data if it's time for them, and reset the corresponding timer if sent - _passthrough.send_attiandrng = true; // next iteration, send attitude b/c it needs frequent updates to remain smooth - uint32_t now = AP_HAL::millis(); - if ((now - _passthrough.params_timer) >= 1000) { - send_uint32(DIY_FIRST_ID+7, calc_param()); - _passthrough.params_timer = AP_HAL::millis(); - return; - } - if ((now - _passthrough.ap_status_timer) >= 500) { - if (gcs().vehicle_initialised()) { // send ap status only once vehicle has been initialised - send_uint32(DIY_FIRST_ID+1, calc_ap_status()); - _passthrough.ap_status_timer = AP_HAL::millis(); - return; - } - } - if ((now - _passthrough.batt_timer) >= 1000) { - send_uint32(DIY_FIRST_ID+3, calc_batt(0)); - _passthrough.batt_timer = AP_HAL::millis(); - return; - } - if (AP::battery().num_instances() > 1) { - if ((now - _passthrough.batt_timer2) >= 1000) { - send_uint32(DIY_FIRST_ID+8, calc_batt(1)); - _passthrough.batt_timer2 = AP_HAL::millis(); - return; - } - } - if ((now - _passthrough.gps_status_timer) >= 1000) { - send_uint32(DIY_FIRST_ID+2, calc_gps_status()); - _passthrough.gps_status_timer = AP_HAL::millis(); - return; - } - if ((now - _passthrough.home_timer) >= 500) { - send_uint32(DIY_FIRST_ID+4, calc_home()); - _passthrough.home_timer = AP_HAL::millis(); - return; - } - if ((now - _passthrough.velandyaw_timer) >= 500) { - send_uint32(DIY_FIRST_ID+5, calc_velandyaw()); - _passthrough.velandyaw_timer = AP_HAL::millis(); - return; - } - if ((now - _passthrough.gps_latlng_timer) >= 1000) { - send_uint32(GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude - if (!_passthrough.send_latitude) { // we've cycled and sent one each of longitude then latitude, so reset the timer - _passthrough.gps_latlng_timer = AP_HAL::millis(); - } - return; - } - } - // if nothing else needed to be sent, send attitude (roll, pitch) and range data - send_uint32(DIY_FIRST_ID+6, calc_attiandrng()); + if (prev_byte == START_STOP_SPORT) { + if (_passthrough.new_byte == SENSOR_ID_28) { // byte 0x7E is the header of each poll request + update_avg_packet_rate(); + passthrough_wfq_adaptive_scheduler(prev_byte); } } } @@ -211,10 +284,10 @@ void AP_Frsky_Telem::send_SPort(void) case SENSOR_ID_FAS: switch (_SPort.fas_call) { case 0: - send_uint32(DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining + send_uint32(SPORT_DATA_FRAME, DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining break; case 1: - send_uint32(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage + send_uint32(SPORT_DATA_FRAME, DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage break; case 2: { @@ -222,9 +295,10 @@ void AP_Frsky_Telem::send_SPort(void) if (!_battery.current_amps(current)) { current = 0; } - send_uint32(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption + send_uint32(SPORT_DATA_FRAME, DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption break; - } + } + break; } if (_SPort.fas_call++ > 2) _SPort.fas_call = 0; break; @@ -232,37 +306,37 @@ void AP_Frsky_Telem::send_SPort(void) switch (_SPort.gps_call) { case 0: calc_gps_position(); // gps data is not recalculated until all of it has been sent - send_uint32(DATA_ID_GPS_LAT_BP, _gps.latdddmm); // send gps lattitude degree and minute integer part + send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LAT_BP, _gps.latdddmm); // send gps lattitude degree and minute integer part break; case 1: - send_uint32(DATA_ID_GPS_LAT_AP, _gps.latmmmm); // send gps lattitude minutes decimal part + send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LAT_AP, _gps.latmmmm); // send gps lattitude minutes decimal part break; case 2: - send_uint32(DATA_ID_GPS_LAT_NS, _gps.lat_ns); // send gps North / South information + send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LAT_NS, _gps.lat_ns); // send gps North / South information break; case 3: - send_uint32(DATA_ID_GPS_LONG_BP, _gps.londddmm); // send gps longitude degree and minute integer part + send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LONG_BP, _gps.londddmm); // send gps longitude degree and minute integer part break; case 4: - send_uint32(DATA_ID_GPS_LONG_AP, _gps.lonmmmm); // send gps longitude minutes decimal part + send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LONG_AP, _gps.lonmmmm); // send gps longitude minutes decimal part break; case 5: - send_uint32(DATA_ID_GPS_LONG_EW, _gps.lon_ew); // send gps East / West information + send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LONG_EW, _gps.lon_ew); // send gps East / West information break; case 6: - send_uint32(DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part + send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part break; case 7: - send_uint32(DATA_ID_GPS_SPEED_AP, _gps.speed_in_centimeter); // send gps speed decimal part + send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_SPEED_AP, _gps.speed_in_centimeter); // send gps speed decimal part break; case 8: - send_uint32(DATA_ID_GPS_ALT_BP, _gps.alt_gps_meters); // send gps altitude integer part + send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_ALT_BP, _gps.alt_gps_meters); // send gps altitude integer part break; case 9: - send_uint32(DATA_ID_GPS_ALT_AP, _gps.alt_gps_cm); // send gps altitude decimals + send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_ALT_AP, _gps.alt_gps_cm); // send gps altitude decimals break; case 10: - send_uint32(DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS + send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS break; } if (_SPort.gps_call++ > 10) _SPort.gps_call = 0; @@ -271,10 +345,10 @@ void AP_Frsky_Telem::send_SPort(void) switch (_SPort.vario_call) { case 0 : calc_nav_alt(); // nav altitude is not recalculated until all of it has been sent - send_uint32(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send altitude integer part + send_uint32(SPORT_DATA_FRAME, DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send altitude integer part break; case 1: - send_uint32(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send altitude decimal part + send_uint32(SPORT_DATA_FRAME, DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send altitude decimal part break; } if (_SPort.vario_call++ > 1) _SPort.vario_call = 0; @@ -282,10 +356,10 @@ void AP_Frsky_Telem::send_SPort(void) case SENSOR_ID_SP2UR: switch (_SPort.various_call) { case 0 : - send_uint32(DATA_ID_TEMP2, (uint16_t)(AP::gps().num_sats() * 10 + AP::gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t) + send_uint32(SPORT_DATA_FRAME, DATA_ID_TEMP2, (uint16_t)(AP::gps().num_sats() * 10 + AP::gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t) break; case 1: - send_uint32(DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode + send_uint32(SPORT_DATA_FRAME, DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode break; } if (_SPort.various_call++ > 1) _SPort.various_call = 0; @@ -319,7 +393,7 @@ void AP_Frsky_Telem::send_D(void) if (!_battery.current_amps(current)) { current = 0; } - send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption + send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption calc_nav_alt(); send_uint16(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send nav altitude integer part send_uint16(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send nav altitude decimal part @@ -423,9 +497,9 @@ void AP_Frsky_Telem::send_byte(uint8_t byte) /* * send one uint32 frame of FrSky data - for FrSky SPort protocol (X-receivers) */ -void AP_Frsky_Telem::send_uint32(uint16_t id, uint32_t data) +void AP_Frsky_Telem::send_uint32(uint8_t frame, uint16_t id, uint32_t data) { - send_byte(0x10); // DATA_FRAME + send_byte(frame); // frame type uint8_t *bytes = (uint8_t*)&id; send_byte(bytes[0]); // LSB send_byte(bytes[1]); // MSB @@ -483,7 +557,21 @@ bool AP_Frsky_Telem::get_next_msg_chunk(void) } } - if (_msg_chunk.repeats++ > 2) { // repeat each message chunk 3 times to ensure transmission + // repeat each message chunk 3 times to ensure transmission + // on slow links reduce the number of duplicate chunks + uint8_t extra_chunks = 2; + + if (_passthrough.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) { + // with 1 or 2 extra frsky sensors on the bus + // send messages twice + extra_chunks = 1; + } + + if (_msg_chunk.repeats++ > extra_chunks ) { _msg_chunk.repeats = 0; if (_msg_chunk.char_index == 0) { // if we're ready for the next message _statustext_queue.remove(0); @@ -740,6 +828,8 @@ uint32_t AP_Frsky_Telem::calc_ap_status(void) ap_status |= (uint8_t)(AP_Notify::flags.ekf_bad)<printf("flying=%d\n",AP_Notify::flags.flying); + //hal.console->printf("ap_status=%08X\n",ap_status); return ap_status; } diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index ad1415882d..9930a7016a 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -63,7 +63,7 @@ for FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers) #define START_STOP_SPORT 0x7E #define BYTESTUFF_SPORT 0x7D - +#define SPORT_DATA_FRAME 0x10 /* for FrSky SPort Passthrough */ @@ -107,8 +107,8 @@ 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: @@ -138,9 +138,8 @@ private: uint32_t check_sensor_status_timer; uint32_t check_ekf_status_timer; uint8_t _paramID; - + ObjectArray _statustext_queue; - struct { @@ -157,22 +156,36 @@ private: uint16_t speed_in_centimeter; } _gps; - struct + struct PACKED { + 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; - bool send_attiandrng; - bool send_latitude; - bool send_chunk; - uint32_t params_timer; - uint32_t ap_status_timer; - uint32_t batt_timer; - uint32_t batt_timer2; - uint32_t gps_status_timer; - uint32_t home_timer; - uint32_t velandyaw_timer; - uint32_t gps_latlng_timer; } _passthrough; + + struct + { + const uint32_t packet_min_period[TIME_SLOT_MAX] = { + 0, //0x5000 text, no rate limiter + 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 { bool sport_status; @@ -195,6 +208,9 @@ private: uint8_t char_index; // index of which character to get in the message } _msg_chunk; + // passthrough WFQ scheduler + void update_avg_packet_rate(); + void passthrough_wfq_adaptive_scheduler(uint8_t prev_byte); // main transmission function when protocol is FrSky SPort Passthrough (OpenTX) void send_SPort_Passthrough(void); // main transmission function when protocol is FrSky SPort @@ -203,13 +219,12 @@ private: 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 calc_crc(uint8_t byte); void send_crc(void); void send_byte(uint8_t value); - void send_uint32(uint16_t id, uint32_t data); void send_uint16(uint16_t id, uint16_t data); + 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);