From c0663da918dd1266e8f1629422e674ccbe4b7713 Mon Sep 17 00:00:00 2001 From: yaapu Date: Mon, 1 Nov 2021 18:13:21 +0100 Subject: [PATCH] AP_RCTelemetry: added ExpressLRS support --- libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp | 129 ++++++++++++++------- libraries/AP_RCTelemetry/AP_CRSF_Telem.h | 23 +++- 2 files changed, 107 insertions(+), 45 deletions(-) diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp index 9d9ac6eec0..2bb2213090 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp @@ -39,6 +39,9 @@ #endif extern const AP_HAL::HAL& hal; +const uint8_t AP_CRSF_Telem::PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE; +const uint8_t AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE; +const uint8_t AP_CRSF_Telem::CRSF_RX_DEVICE_PING_MAX_RETRY; AP_CRSF_Telem *AP_CRSF_Telem::singleton; @@ -97,7 +100,7 @@ void AP_CRSF_Telem::setup_custom_telemetry() // check if passthru already assigned const int8_t frsky_port = AP::serialmanager().find_portnum(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough,0); if (frsky_port != -1) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "CRSF: passthrough telemetry conflict on SERIAL%d",frsky_port); + gcs().send_text(MAV_SEVERITY_CRITICAL, "%s: passthrough telemetry conflict on SERIAL%d", get_protocol_string(), frsky_port); _custom_telem.init_done = true; return; } @@ -112,15 +115,13 @@ void AP_CRSF_Telem::setup_custom_telemetry() return; } - // setup the frsky scheduler for crossfire + // setup the frsky scheduler for crossfire and elrs passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::GPS_LAT); passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::GPS_LON); passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::TEXT); passthrough->set_scheduler_entry_min_period(AP_Frsky_SPort_Passthrough::ATTITUDE, 350); // 3Hz // setup the crossfire scheduler for custom telemetry - set_scheduler_entry(BATTERY, 1000, 1000); // 1Hz - set_scheduler_entry(ATTITUDE, 1000, 1000); // 1Hz set_scheduler_entry(FLIGHT_MODE, 1200, 2000); // 0.5Hz set_scheduler_entry(HEARTBEAT, 2000, 5000); // 0.2Hz @@ -128,7 +129,8 @@ void AP_CRSF_Telem::setup_custom_telemetry() // setup custom telemetry for current rf_mode update_custom_telemetry_rates(_telem_rf_mode); - gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: custom telem init done, fw %d.%02d", _crsf_version.major, _crsf_version.minor); + gcs().send_text(MAV_SEVERITY_DEBUG,"%s: custom telem init done, fw %d.%02d", get_protocol_string(), _crsf_version.major, _crsf_version.minor); + _custom_telem.init_done = true; } @@ -140,15 +142,28 @@ void AP_CRSF_Telem::update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_ } 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(PASSTHROUGH, 100, 100); // 10Hz - set_scheduler_entry(STATUS_TEXT, 200, 750); // 1.5Hz + set_scheduler_entry(GPS, 550, 500); // 2.0Hz + set_scheduler_entry(PASSTHROUGH, 100, 100); // 8Hz + set_scheduler_entry(STATUS_TEXT, 200, 750); // 1.5Hz } else { - // custom telemetry for low data rates - set_scheduler_entry(GPS, 550, 1000); // 1.0Hz - set_scheduler_entry(PASSTHROUGH, 500, 3000); // 0.3Hz - set_scheduler_entry(STATUS_TEXT, 600, 2000); // 0.5Hz + // standard telemetry for low data rates + set_scheduler_entry(BATTERY, 1000, 2000); // 0.5Hz + set_scheduler_entry(ATTITUDE, 1000, 3000); // 0.33Hz + if (_crsf_version.is_elrs) { + // ELRS custom telemetry for low data rates + set_scheduler_entry(GPS, 550, 1000); // 1.0Hz + set_scheduler_entry(PASSTHROUGH, 350, 500); // 2.0Hz + set_scheduler_entry(STATUS_TEXT, 500, 2000); // 0.5Hz + } else { + // CRSF custom telemetry for low data rates + set_scheduler_entry(GPS, 550, 1000); // 1.0Hz + set_scheduler_entry(PASSTHROUGH, 500, 3000); // 0.3Hz + set_scheduler_entry(STATUS_TEXT, 600, 2000); // 0.5Hz + } } } @@ -168,22 +183,25 @@ void AP_CRSF_Telem::process_rf_mode_changes() } // warn the user if their setup is sub-optimal if (_telem_last_report_ms == 0 && !uart->is_dma_enabled()) { - gcs().send_text(MAV_SEVERITY_WARNING, "CRSF: running on non-DMA serial port"); + gcs().send_text(MAV_SEVERITY_WARNING, "%s: running on non-DMA serial port", get_protocol_string()); } // note if option was set to show LQ in place of RSSI bool current_lq_as_rssi_active = bool(rc().use_crsf_lq_as_rssi()); if(_telem_last_report_ms == 0 || _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, "CRSF: RSSI now displays %s", current_lq_as_rssi_active ? " as LQ" : "normally"); + gcs().send_text(MAV_SEVERITY_INFO, "%s: RSSI now displays %s", get_protocol_string(), current_lq_as_rssi_active ? " as LQ" : "normally"); } - // report a change in RF mode or a chnage of more than 10Hz if we haven't done so in the last 5s - if ((now - _telem_last_report_ms > 5000) && - (_telem_rf_mode != current_rf_mode || abs(int16_t(_telem_last_avg_rate) - int16_t(_scheduler.avg_packet_rate)) > 25)) { - if (!rc().suppress_crsf_message()) { - gcs().send_text(MAV_SEVERITY_INFO, "CRSFv%d: RF mode %d, rate is %dHz", uint8_t(2 + AP::crsf()->is_crsf_v3_active()), - (uint8_t)current_rf_mode, _scheduler.avg_packet_rate); + 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().suppress_crsf_message() && (_telem_rf_mode != current_rf_mode || abs(int16_t(_telem_last_avg_rate) - int16_t(_scheduler.avg_packet_rate)) > 25)) { + gcs().send_text(MAV_SEVERITY_INFO, "%s: RF Mode %d, telemetry rate is %dHz", get_protocol_string(), uint8_t(current_rf_mode) - (_crsf_version.is_elrs ? uint8_t(AP_RCProtocol_CRSF::RFMode::ELRS_RF_MODE_4HZ) : 0), get_telemetry_rate()); } - update_custom_telemetry_rates(current_rf_mode); + // 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; _telem_rf_mode = current_rf_mode; _telem_last_avg_rate = _scheduler.avg_packet_rate; _telem_last_report_ms = now; @@ -193,7 +211,8 @@ void AP_CRSF_Telem::process_rf_mode_changes() // return custom frame id based on fw version uint8_t AP_CRSF_Telem::get_custom_telem_frame_id() const { - if (!_crsf_version.pending && (_crsf_version.major > 4 || (_crsf_version.major == 4 && _crsf_version.minor >= 6))) { + if (!_crsf_version.pending && + ((_crsf_version.major > 4 || (_crsf_version.major == 4 && _crsf_version.minor >= 6)) || _crsf_version.is_elrs)) { return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM; } return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM_LEGACY; @@ -203,30 +222,33 @@ AP_RCProtocol_CRSF::RFMode AP_CRSF_Telem::get_rf_mode() const { AP_RCProtocol_CRSF* crsf = AP::crsf(); if (crsf == nullptr) { - return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_UNKNOWN; + return AP_RCProtocol_CRSF::RFMode::RF_MODE_UNKNOWN; } if (!_crsf_version.pending && _crsf_version.use_rf_mode) { - return crsf->get_link_status().rf_mode; + if (_crsf_version.is_elrs) { + return static_cast(uint8_t(AP_RCProtocol_CRSF::RFMode::ELRS_RF_MODE_4HZ) + crsf->get_link_status().rf_mode); + } + return static_cast(crsf->get_link_status().rf_mode); } else if (_crsf_version.is_tracer) { return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_250HZ; } /* Note: - - rf mode 2 on UARTS with DMA runs @160Hz - - rf mode 2 on UARTS with no DMA runs @70Hz + - CRSF rf mode 2 on UARTS with DMA runs @160Hz + - CRSF rf mode 2 on UARTS with no DMA runs @70Hz */ if (get_avg_packet_rate() < 40U) { - // no DMA rf mode 1 + // no DMA CRSF rf mode 1 return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ; } if (get_avg_packet_rate() > 120U) { - // DMA rf mode 2 + // DMA CRSF rf mode 2 return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ; } if (get_max_packet_rate() < 120U) { - // no DMA rf mode 2 + // no CRSF DMA rf mode 2 return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ; } return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ; @@ -234,7 +256,26 @@ AP_RCProtocol_CRSF::RFMode AP_CRSF_Telem::get_rf_mode() const bool AP_CRSF_Telem::is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_mode) const { - return rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ || rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_250HZ; + if (!_crsf_version.is_elrs) { + return rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ || rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_250HZ; + } + return get_telemetry_rate() > 30; +} + +uint16_t AP_CRSF_Telem::get_telemetry_rate() const +{ + if (!_crsf_version.is_elrs) { + return get_avg_packet_rate(); + } + AP_RCProtocol_CRSF* crsf = AP::crsf(); + if (crsf == nullptr) { + return get_avg_packet_rate(); + } + // ELRS sends 1 telemetry frame every n RC frames + // the 1:n ratio is user selected + // RC rate is measured by get_avg_packet_rate() + // telemetry rate = air rate - RC rate + return uint16_t(AP_RCProtocol_CRSF::elrs_air_rates[MIN(crsf->get_link_status().rf_mode, 7U)] - get_avg_packet_rate()); } void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text) @@ -316,11 +357,11 @@ bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty) _crsf_version.pending = false; _crsf_version.minor = 0; _crsf_version.major = 0; - gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: RX device ping failed"); + gcs().send_text(MAV_SEVERITY_DEBUG,"%s: RX device ping failed", get_protocol_string()); } else { _pending_request.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER; _pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING; - gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: requesting RX device info"); + gcs().send_text(MAV_SEVERITY_DEBUG,"%s: requesting RX device info", get_protocol_string()); } } return _pending_request.frame_type > 0; @@ -371,7 +412,8 @@ void AP_CRSF_Telem::process_packet(uint8_t idx) } else { // on slower links we pack many passthrough // frames in a single crossfire one (up to 9) - get_multi_packet_passthrough_telem_data(); + const uint8_t size = _crsf_version.is_elrs ? 3 : AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE; + get_multi_packet_passthrough_telem_data(size); } break; case STATUS_TEXT: @@ -551,6 +593,10 @@ void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info) const uint8_t offset = strnlen((char*)info->payload,42U); if (strncmp((char*)info->payload, "Tracer", 6) == 0) { _crsf_version.is_tracer = true; + } else if (strncmp((char*)&info->payload[offset+1], "ELRS", 4) == 0) { + // ELRS magic number is ELRS encoded in the serial number + // 0x45 'E' 0x4C 'L' 0x52 'R' 0x53 'S' + _crsf_version.is_elrs = true; } /* fw major ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 3rd byte of sw id = 11bytes @@ -560,7 +606,7 @@ void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info) _crsf_version.minor = info->payload[offset+12]; // should we use rf_mode reported by link statistics? - if (!_crsf_version.is_tracer && (_crsf_version.major > 3 || (_crsf_version.major == 3 && _crsf_version.minor >= 72))) { + if (_crsf_version.is_elrs || (!_crsf_version.is_tracer && (_crsf_version.major > 3 || (_crsf_version.major == 3 && _crsf_version.minor >= 72)))) { _crsf_version.use_rf_mode = true; } @@ -1263,7 +1309,7 @@ void AP_CRSF_Telem::calc_status_text() if (!_statustext.available) { WITH_SEMAPHORE(_statustext.sem); // check link speed - if (!is_high_speed_telemetry(_telem_rf_mode)) { + if (!_crsf_version.is_elrs && !is_high_speed_telemetry(_telem_rf_mode)) { // keep only warning/error/critical/alert/emergency status text messages bool got_message = false; while (_statustext.queue.pop(_statustext.next)) { @@ -1318,22 +1364,23 @@ void AP_CRSF_Telem::get_single_packet_passthrough_telem_data() whenever possible we use smaller frames for they have a higher "chance" of being transmitted by the crossfire RX scheduler. */ -void AP_CRSF_Telem::get_multi_packet_passthrough_telem_data() +void AP_CRSF_Telem::get_multi_packet_passthrough_telem_data(uint8_t size) { + size = MIN(size, AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE); _telem_pending = false; uint8_t count = 0; - AP_Frsky_SPort::sport_packet_t buffer[PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE] {}; + AP_Frsky_SPort::sport_packet_t buffer[AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE] {}; // we request a PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE packet array, i.e. 9 packets - if (!AP_Frsky_Telem::get_telem_data(buffer, count, ARRAY_SIZE(buffer))) { + if (!AP_Frsky_Telem::get_telem_data(buffer, count, size)) { return; } _telem.bcast.custom_telem.multi_packet_passthrough.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_MULTI_PACKET_PASSTHROUGH; for (uint8_t idx=0; idxis_crsf_v3_active()) { + return "CRSFv3"; + } + return "CRSFv2"; + } + }; // Process a frame from the CRSF protocol decoder static bool process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data); // process any changed settings and schedule for transmission @@ -263,7 +275,7 @@ private: void process_packet(uint8_t idx) override; void adjust_packet_weight(bool queue_empty) override; void setup_custom_telemetry(); - void update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_mode); + void update_custom_telemetry_rates(const AP_RCProtocol_CRSF::RFMode rf_mode); void calc_parameter_ping(); void calc_heartbeat(); @@ -281,11 +293,12 @@ private: void update_params(); void update_vtx_params(); void get_single_packet_passthrough_telem_data(); - void get_multi_packet_passthrough_telem_data(); + void get_multi_packet_passthrough_telem_data(uint8_t size = PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE); void calc_status_text(); void process_rf_mode_changes(); uint8_t get_custom_telem_frame_id() const; AP_RCProtocol_CRSF::RFMode get_rf_mode() const; + uint16_t get_telemetry_rate() const; bool is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_mode) const; void process_vtx_frame(VTXFrame* vtx); @@ -315,6 +328,7 @@ private: uint32_t _telem_last_report_ms; uint16_t _telem_last_avg_rate; + bool _telem_is_high_speed; bool _telem_pending; bool _enable_telemetry; @@ -330,6 +344,7 @@ private: bool use_rf_mode; bool is_tracer; bool pending = true; + bool is_elrs; } _crsf_version; struct {