From ba6842d19e700b7279e8cfd26929fa0c92a3af49 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 22 Nov 2022 20:23:40 +0000 Subject: [PATCH] AP_RCTelemetry: report CRSF link rate rather than mode. Encode actual protocol being used cleanup is_elrs() and version numbers --- libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp | 48 +++++++++++++--------- libraries/AP_RCTelemetry/AP_CRSF_Telem.h | 21 ++++------ 2 files changed, 37 insertions(+), 32 deletions(-) diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp index e30d046293..1dd14bf11f 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp @@ -158,7 +158,7 @@ void AP_CRSF_Telem::update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_ // 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) { + if (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 @@ -207,7 +207,8 @@ bool AP_CRSF_Telem::process_rf_mode_changes() 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()); + gcs().send_text(MAV_SEVERITY_INFO, "%s: Link rate %dHz, Telemetry rate %dHz", + get_protocol_string(), crsf->get_link_rate(_crsf_version.protocol), get_telemetry_rate()); } // tune the scheduler based on telemetry speed high/low transitions if (_telem_is_high_speed != is_high_speed) { @@ -228,7 +229,8 @@ bool AP_CRSF_Telem::process_rf_mode_changes() 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)) || _crsf_version.is_elrs)) { + ((_crsf_version.major > 4 || (_crsf_version.major == 4 && _crsf_version.minor >= 6)) + || is_elrs())) { return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM; } return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM_LEGACY; @@ -242,11 +244,11 @@ AP_RCProtocol_CRSF::RFMode AP_CRSF_Telem::get_rf_mode() const } if (!_crsf_version.pending && _crsf_version.use_rf_mode) { - if (_crsf_version.is_elrs) { + if (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) { + } else if (is_tracer()) { return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_250HZ; } @@ -272,7 +274,7 @@ 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 { - if (!_crsf_version.is_elrs) { + if (_crsf_version.protocol != AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_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; @@ -280,7 +282,7 @@ bool AP_CRSF_Telem::is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_ uint16_t AP_CRSF_Telem::get_telemetry_rate() const { - if (!_crsf_version.is_elrs) { + if (_crsf_version.protocol != AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_ELRS) { return get_avg_packet_rate(); } AP_RCProtocol_CRSF* crsf = AP::crsf(); @@ -291,7 +293,7 @@ uint16_t AP_CRSF_Telem::get_telemetry_rate() const // 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()); + return crsf->get_link_rate(_crsf_version.protocol) - get_avg_packet_rate(); } void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text) @@ -437,7 +439,7 @@ 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) - const uint8_t size = _crsf_version.is_elrs ? 3 : AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE; + const uint8_t size = is_elrs() ? 3 : AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE; get_multi_packet_passthrough_telem_data(size); } break; @@ -634,21 +636,28 @@ void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info) // get the terminator of the device name string const uint8_t offset = strnlen((char*)info->payload,42U); if (strncmp((char*)info->payload, "Tracer", 6) == 0) { - _crsf_version.is_tracer = true; + _crsf_version.protocol = AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_TRACER; } 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; + _crsf_version.protocol = AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_ELRS; + } + + if (!is_elrs()) { + /* + fw major ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 3rd byte of sw id = 11bytes + fw minor ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 4th byte of sw id = 12bytes + */ + _crsf_version.major = info->payload[offset+11]; + _crsf_version.minor = info->payload[offset+12]; + } else { + // ELRS does not populate the version field so cook up something sensible + _crsf_version.major = 1; + _crsf_version.minor = 0; } - /* - fw major ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 3rd byte of sw id = 11bytes - fw minor ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 4th byte of sw id = 12bytes - */ - _crsf_version.major = info->payload[offset+11]; - _crsf_version.minor = info->payload[offset+12]; // should we use rf_mode reported by link statistics? - if (_crsf_version.is_elrs || (!_crsf_version.is_tracer && (_crsf_version.major > 3 || (_crsf_version.major == 3 && _crsf_version.minor >= 72)))) { + if (is_elrs() || (!is_tracer() && (_crsf_version.major > 3 || (_crsf_version.major == 3 && _crsf_version.minor >= 72)))) { _crsf_version.use_rf_mode = true; } @@ -1352,7 +1361,8 @@ void AP_CRSF_Telem::calc_status_text() if (!_statustext.available) { WITH_SEMAPHORE(_statustext.sem); // check link speed - if (!_crsf_version.is_elrs && !is_high_speed_telemetry(_telem_rf_mode)) { + if (_crsf_version.protocol != AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_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)) { diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h index 6d01a0f1f2..5eda87aee6 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h @@ -232,17 +232,13 @@ public: }; // get the protocol string - const char* get_protocol_string() const { - if (_crsf_version.is_elrs) { - return "ELRS"; - } else { - const AP_RCProtocol_CRSF* crsf = AP::crsf(); - if (crsf && crsf->is_crsf_v3_active()) { - return "CRSFv3"; - } - return "CRSFv2"; - } - }; + const char* get_protocol_string() const { return AP::crsf()->get_protocol_string(_crsf_version.protocol); } + + // is the current protocol ELRS? + bool is_elrs() const { return _crsf_version.protocol == AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_ELRS; } + // is the current protocol Tracer? + bool is_tracer() const { return _crsf_version.protocol == AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_TRACER; } + // 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 @@ -346,9 +342,8 @@ private: uint8_t major; uint8_t retry_count; bool use_rf_mode; - bool is_tracer; + AP_RCProtocol_CRSF::ProtocolType protocol; bool pending = true; - bool is_elrs; } _crsf_version; struct {