AP_Frsky_Telem: added a parameter to set the default FRSky sensor ID for passthrough telemetry

libraries/AP_Frsky_Telem/AP_Frsky_Backend.h

@@ -138,7 +138,6 @@ protected:
     static const uint8_t SENSOR_ID_GPS             = 0x83; // Sensor ID  3
     static const uint8_t SENSOR_ID_RPM             = 0xE4; // Sensor ID  4
     static const uint8_t SENSOR_ID_SP2UR           = 0xC6; // Sensor ID  6
-    static const uint8_t SENSOR_ID_27              = 0x1B; // Sensor ID 27
 
 private:
 

libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp

@@ -38,6 +38,12 @@ const AP_Param::GroupInfo AP_Frsky_Parameters::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("DNLINK2_ID",  3, AP_Frsky_Parameters, _dnlink2_id, 7),
 
+    // @Param: DNLINK_ID
+    // @DisplayName: Default downlink sensor id
+    // @Description: Change the default downlink sensor id (SPort only)
+    // @Values: -1:Disable,7:7,8:8,9:9,10:10,11:11,12:12,13:13,14:14,15:15,16:16,17:17,18:18,19:19,20:20,21:21,22:22,23:23,24:24,25:25,26:26,27:27
+    // @User: Advanced
+    AP_GROUPINFO("DNLINK_ID",  4, AP_Frsky_Parameters, _dnlink_id, 27),
     AP_GROUPEND
 };
 

libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h

@@ -34,6 +34,7 @@ public:
 private:
     // settable parameters
     AP_Int8 _uplink_id;
+    AP_Int8 _dnlink_id;
     AP_Int8 _dnlink1_id;
     AP_Int8 _dnlink2_id;
 };

libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp

@@ -121,6 +121,9 @@ void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void)
     set_scheduler_entry(RPM, 300, 330);         // 0x500A rpm sensors 1 and 2
     set_scheduler_entry(TERRAIN, 700, 500);     // 0x500B terrain data
     set_scheduler_entry(UDATA, 5000, 200);      // user data
+
+    // initialize default sport sensor ID
+    set_sensor_id(_frsky_parameters->_dnlink_id, downlink_sensor_id);
 #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
     set_scheduler_entry(MAV, 35, 25);           // mavlite
     // initialize sport sensor IDs
@@ -189,8 +192,8 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty)
         break;
     case GPS_LAT:
     case GPS_LON:
-        // force gps coords to use sensor 0x1B, always send when used with external data
-        packet_ready = _use_external_data || (_passthrough.new_byte == SENSOR_ID_27);
+        // force gps coords to use default sensor ID, always send when used with external data
+        packet_ready = _use_external_data || (_passthrough.new_byte == downlink_sensor_id);
         break;
     case AP_STATUS:
         packet_ready = gcs().vehicle_initialised();
@@ -502,7 +505,7 @@ bool AP_Frsky_SPort_Passthrough::is_passthrough_byte(const uint8_t byte) const
         return true;
     }
 #endif
-    return byte == SENSOR_ID_27;
+    return byte == downlink_sensor_id;
 }
 
 /*

libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h

@@ -124,6 +124,8 @@ private:
         uint8_t char_index; // index of which character to get in the message
     } _msg_chunk;
 
+    // passthrough default sensor id
+    uint8_t downlink_sensor_id = 0x1B;
 
 #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
     // bidirectional sport telemetry