AP_RCTelemetry: correct VTX power settings and pass parameter requests more quickly

return parameter ids when CRSF folder request is made
turn off telemetry while processing parameters

libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp

@@ -32,7 +32,7 @@
 
 #if HAL_CRSF_TELEM_ENABLED
 
-// #define CRSF_DEBUG
+//#define CRSF_DEBUG
 #ifdef CRSF_DEBUG
 # define debug(fmt, args...)	hal.console->printf("CRSF: " fmt "\n", ##args)
 #else
@@ -74,7 +74,7 @@ void AP_CRSF_Telem::setup_wfq_scheduler(void)
 
     // CSRF telemetry rate is 150Hz (4ms) max, so these rates must fit
     add_scheduler_entry(50, 100);   // heartbeat        10Hz
-    add_scheduler_entry(50, 50);    // parameters       20Hz (generally not active unless requested by the TX)
+    add_scheduler_entry(5, 20);     // parameters       50Hz (generally not active unless requested by the TX)
     add_scheduler_entry(50, 120);   // Attitude and compass 8Hz
     add_scheduler_entry(200, 1000); // VTX parameters    1Hz
     add_scheduler_entry(1300, 500); // battery           2Hz
@@ -232,24 +232,27 @@ void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text)
 
 void AP_CRSF_Telem::enter_scheduler_params_mode()
 {
-    set_scheduler_entry(HEARTBEAT, 50, 100);            // heartbeat        10Hz
-    set_scheduler_entry(ATTITUDE, 50, 120);             // Attitude and compass 8Hz
-    set_scheduler_entry(BATTERY, 1300, 500);            // battery           2Hz
-    set_scheduler_entry(GPS, 550, 280);                 // GPS               3Hz
-    set_scheduler_entry(FLIGHT_MODE, 550, 500);         // flight mode       2Hz
+    debug("parameter passthrough enabled");
+    set_scheduler_entry(HEARTBEAT, 50, 200);            // heartbeat        5Hz
 
+    disable_scheduler_entry(ATTITUDE);
+    disable_scheduler_entry(BATTERY);
+    disable_scheduler_entry(GPS);
+    disable_scheduler_entry(FLIGHT_MODE);
     disable_scheduler_entry(PASSTHROUGH);
     disable_scheduler_entry(STATUS_TEXT);
 }
 
 void AP_CRSF_Telem::exit_scheduler_params_mode()
 {
+    debug("parameter passthrough disabled");
     // 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
 
+    enable_scheduler_entry(ATTITUDE);
+    enable_scheduler_entry(BATTERY);
+    enable_scheduler_entry(GPS);
+    enable_scheduler_entry(FLIGHT_MODE);
     enable_scheduler_entry(PASSTHROUGH);
     enable_scheduler_entry(STATUS_TEXT);
 
@@ -425,10 +428,10 @@ void AP_CRSF_Telem::process_vtx_frame(VTXFrame* vtx) {
             apvtx.set_power_mw(25);
             break;
         case 1:
-            apvtx.set_power_mw(200);
+            apvtx.set_power_mw(100);
             break;
         case 2:
-            apvtx.set_power_mw(500);
+            apvtx.set_power_mw(400);
             break;
         case 3:
             apvtx.set_power_mw(800);
@@ -537,8 +540,8 @@ void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info)
 
 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,
-    //    read_frame->param_number, read_frame->param_chunk);
+    debug("process_param_read_frame: %d -> %d for %d[%d]", read_frame->origin, read_frame->destination,
+        read_frame->param_num, read_frame->param_chunk);
     if (read_frame->destination != 0 && read_frame->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
         return; // request was not for us
     }
@@ -554,6 +557,10 @@ void AP_CRSF_Telem::update()
 
 void AP_CRSF_Telem::update_params()
 {
+    uint32_t now = AP_HAL::millis();
+    // reset parameter passthrough timeout
+    _custom_telem.params_mode_start_ms = now;
+
     // handle general parameter requests
     switch (_pending_request.frame_type) {
     case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING:
@@ -771,7 +778,7 @@ void AP_CRSF_Telem::calc_device_info() {
     const AP_FWVersion &fwver = AP::fwversion();
     // write out the name with version, max width is 60 - 18 = the meaning of life
     int32_t n = strlen(fwver.fw_string);
-    strncpy((char*)_telem.ext.info.payload, fwver.fw_string, 41);
+    strncpy((char*)_telem.ext.info.payload, fwver.fw_short_string, 41);
     n = MIN(n + 1, 42);
 
     put_be32_ptr(&_telem.ext.info.payload[n], // serial number
@@ -813,15 +820,41 @@ void AP_CRSF_Telem::calc_device_ping() {
 // return parameter information
 void AP_CRSF_Telem::calc_parameter() {
 #if OSD_PARAM_ENABLED
+    _telem.ext.param_entry.header.destination = _param_request.origin;
+    _telem.ext.param_entry.header.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
+    size_t idx = 0;
+
+    // root folder request
+    if (_param_request.param_num == 0) {
+        _telem.ext.param_entry.header.param_num = 0;
+        _telem.ext.param_entry.header.chunks_left = 0;
+        _telem.ext.param_entry.payload[idx++] = 0; // parent folder
+        _telem.ext.param_entry.payload[idx++] = ParameterType::FOLDER; // type
+        _telem.ext.param_entry.payload[idx++] = 'r'; // "root" name
+        _telem.ext.param_entry.payload[idx++] = 'o';
+        _telem.ext.param_entry.payload[idx++] = 'o';
+        _telem.ext.param_entry.payload[idx++] = 't';
+        _telem.ext.param_entry.payload[idx++] = 0; // null terminator
+
+        // write out all of the ids we are going to send
+        for (uint8_t i = 0; i < AP_OSD_ParamScreen::NUM_PARAMS * AP_OSD_NUM_PARAM_SCREENS; i++) {
+            _telem.ext.param_entry.payload[idx++] = i + 1;
+        }
+        _telem.ext.param_entry.payload[idx] = 0xFF; // terminator
+
+        _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + 1 + idx;
+        _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
+        _pending_request.frame_type = 0;
+        _telem_pending = true;
+        return;
+    }
+
     AP_OSD* osd = AP::osd();
 
     if (osd == nullptr) {
         return;
     }
 
-    _telem.ext.param_entry.header.destination = _param_request.origin;
-    _telem.ext.param_entry.header.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
-
     AP_OSD_ParamSetting* param = osd->get_setting((_param_request.param_num - 1) / AP_OSD_ParamScreen::NUM_PARAMS,
         (_param_request.param_num - 1) % AP_OSD_ParamScreen::NUM_PARAMS);
 
@@ -836,7 +869,6 @@ void AP_CRSF_Telem::calc_parameter() {
         return;
     }
 #endif
-    size_t idx = 0;
     _telem.ext.param_entry.header.chunks_left = 0;
     _telem.ext.param_entry.payload[idx++] = 0; // parent folder
     idx++;  // leave a gap for the type