AP_RCTelemetry: add baud rate change command frame processing.

report CRSF version when rate changes

libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp

@@ -82,6 +82,7 @@ void AP_CRSF_Telem::setup_wfq_scheduler(void)
     add_scheduler_entry(550, 500);  // flight mode       2Hz
     add_scheduler_entry(5000, 100); // passthrough       max 10Hz
     add_scheduler_entry(5000, 500); // status text       max 2Hz
+    add_scheduler_entry(5, 20);     // command          50Hz (generally not active unless requested by the TX)
 }
 
 void AP_CRSF_Telem::setup_custom_telemetry()
@@ -174,7 +175,8 @@ void AP_CRSF_Telem::process_rf_mode_changes()
     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, "CRSF: RF mode %d, rate is %dHz", (uint8_t)current_rf_mode, _scheduler.avg_packet_rate);
+            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);
         }
         update_custom_telemetry_rates(current_rf_mode);
         _telem_rf_mode = current_rf_mode;
@@ -320,6 +322,8 @@ bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
         return rc().crsf_custom_telemetry();
     case STATUS_TEXT:
         return rc().crsf_custom_telemetry() && !queue_empty;
+    case GENERAL_COMMAND:
+        return _baud_rate_request.pending;
     default:
         return _enable_telemetry;
     }
@@ -365,6 +369,9 @@ void AP_CRSF_Telem::process_packet(uint8_t idx)
         case STATUS_TEXT:
             calc_status_text();
             break;
+        case GENERAL_COMMAND:
+            calc_command_response();
+            break;
         default:
             break;
     }
@@ -404,6 +411,10 @@ bool AP_CRSF_Telem::_process_frame(AP_RCProtocol_CRSF::FrameType frame_type, voi
         process_device_info_frame((ParameterDeviceInfoFrame*)data);
         break;
 
+    case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND:
+        process_command_frame((CommandFrame*)data);
+        break;
+
     default:
         break;
     }
@@ -548,6 +559,34 @@ void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info)
     _crsf_version.pending = false;
 }
 
+// request for a general command
+void AP_CRSF_Telem::process_command_frame(CommandFrame* command)
+{
+    debug("process_command_frame: 0x%x -> 0x%x: 0x%x", command->origin, command->destination, command->payload[0]);
+    if (command->destination != 0 && command->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
+        return; // request was not for us
+    }
+
+    // we are only interested in commands from the RX
+    if (command->origin != 0 && command->origin != AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER) {
+        return;
+    }
+
+    switch (command->payload[0]) {
+        case AP_RCProtocol_CRSF::CRSF_COMMAND_GENERAL_CRSF_SPEED_PROPOSAL: {
+            uint32_t baud_rate = command->payload[2] << 24 | command->payload[3] << 16
+                | command->payload[4] << 8 | command->payload[5];
+            _baud_rate_request.port_id = command->payload[1];
+            _baud_rate_request.valid = AP::crsf()->change_baud_rate(baud_rate);
+            _baud_rate_request.pending = true;
+            debug("requested baud rate change %lu", baud_rate);
+            break;
+        }
+        default:
+            break; // do nothing
+    }
+}
+
 void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_frame)
 {
     debug("process_param_read_frame: %d -> %d for %d[%d]", read_frame->origin, read_frame->destination,
@@ -827,6 +866,33 @@ void AP_CRSF_Telem::calc_device_ping() {
     _telem_pending = true;
 }
 
+// send a command response
+void AP_CRSF_Telem::calc_command_response() {
+    _telem.ext.command.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER;
+    _telem.ext.command.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
+    _telem.ext.command.command_id = AP_RCProtocol_CRSF::CRSF_COMMAND_GENERAL;
+    _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_GENERAL_CRSF_SPEED_RESPONSE;
+    _telem.ext.command.payload[1] = _baud_rate_request.port_id;
+    _telem.ext.command.payload[2] = _baud_rate_request.valid;
+    _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND;
+
+    // calculate command crc
+    uint8_t len = 6;
+    uint8_t* crcptr = &_telem.ext.command.destination;
+    uint8_t crc = crc8_dvb(0, AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND, 0xBA);
+    for (uint8_t i = 0; i < len; i++) {
+        crc = crc8_dvb(crc, crcptr[i], 0xBA);
+    }
+    crcptr[len] = crc;
+    _telem_size = len + 1;
+
+    _pending_request.frame_type = 0;
+    _baud_rate_request.pending = false;
+
+    debug("sent baud rate response: %u", _baud_rate_request.valid);
+    _telem_pending = true;
+}
+
 // return parameter information
 void AP_CRSF_Telem::calc_parameter() {
 #if OSD_PARAM_ENABLED

libraries/AP_RCTelemetry/AP_CRSF_Telem.h

@@ -253,6 +253,7 @@ private:
         FLIGHT_MODE,
         PASSTHROUGH,
         STATUS_TEXT,
+        GENERAL_COMMAND,
         NUM_SENSORS
     };
 
@@ -271,6 +272,7 @@ private:
     void calc_flight_mode();
     void calc_device_info();
     void calc_device_ping();
+    void calc_command_response();
     void calc_parameter();
 #if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
     void calc_text_selection( AP_OSD_ParamSetting* param, uint8_t chunk);
@@ -291,6 +293,7 @@ private:
     void process_param_read_frame(ParameterSettingsReadFrame* read);
     void process_param_write_frame(ParameterSettingsWriteFrame* write);
     void process_device_info_frame(ParameterDeviceInfoFrame* info);
+    void process_command_frame(CommandFrame* command);
 
     // setup ready for passthrough operation
     void setup_wfq_scheduler(void) override;
@@ -334,6 +337,12 @@ private:
         bool params_mode_active;
     } _custom_telem;
 
+    struct {
+        bool pending;
+        bool valid;
+        uint8_t port_id;
+    } _baud_rate_request;
+
     // vtx state
     bool _vtx_freq_update;  // update using the frequency method or not
     bool _vtx_dbm_update; // update using the dbm method or not