From c61570697140633d0fb3d545ebeae87f4c5174d0 Mon Sep 17 00:00:00 2001 From: yaapu Date: Wed, 5 May 2021 10:39:05 +0200 Subject: [PATCH] AP_Frsky_Telem: added a parameter to set the default FRSky sensor ID for passthrough telemetry --- libraries/AP_Frsky_Telem/AP_Frsky_Backend.h | 1 - libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp | 6 ++++++ libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h | 1 + libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp | 9 ++++++--- libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h | 2 ++ 5 files changed, 15 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h index ebd15b8f6a..4dc28efcab 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h +++ b/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: diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp index dc672674c3..d4fa51c7db 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp +++ b/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 }; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h index b4fd2a52a9..6cfc1b1a03 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h +++ b/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; }; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index a8071cf2e8..20d5bd2528 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/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; } /* diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h index 2b7cd35c47..9c94978649 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h +++ b/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