From 870c280946aaaf4defde8e1abb2df5a7731dc416 Mon Sep 17 00:00:00 2001 From: yaapu Date: Mon, 31 May 2021 09:32:14 +0200 Subject: [PATCH] AP_Frsky_Telem: upgraded SERIAL_PROTOCOL 4 telemetry to 2 bytes data IDs This upgrades standard SPort telemetry from older 1byte IDs to newer 2bytes IDs. Older 1 byte data IDs are not supported by the FRSky ETHOS ecosystem so this migration is required to allow ArduPilot to seamlessly work with ETHOS. This migration requires users to do a new sensor discovery in OpenTX. --- libraries/AP_Frsky_Telem/AP_Frsky_Backend.h | 16 ++++++++-- libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp | 33 ++++++++------------- 2 files changed, 26 insertions(+), 23 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h index 15f80ace70..ebd15b8f6a 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h @@ -102,16 +102,28 @@ protected: static const uint8_t DATA_ID_GPS_LONG_EW = 0x22; static const uint8_t DATA_ID_GPS_LAT_NS = 0x23; static const uint8_t DATA_ID_CURRENT = 0x28; - static const uint8_t DATA_ID_VARIO = 0x30; static const uint8_t DATA_ID_VFAS = 0x39; static const uint8_t START_STOP_D = 0x5E; static const uint8_t BYTESTUFF_D = 0x5D; - // FrSky data IDs; + /* + for FrSky X protocol (X-receivers) + */ + // FrSky 2 bytes DATA IDs; + static const uint16_t ALT_ID = 0x010F; + static const uint16_t VARIO_ID = 0x011F; + static const uint16_t CURR_ID = 0x020F; + static const uint16_t VFAS_ID = 0x021F; + static const uint16_t TEMP1_ID = 0x040F; + static const uint16_t TEMP2_ID = 0x041F; static const uint16_t RPM1_ID = 0x050E; static const uint16_t RPM2_ID = 0x050F; + static const uint16_t FUEL_ID = 0x060F; static const uint16_t GPS_LONG_LATI_FIRST_ID = 0x0800; + static const uint16_t GPS_ALT_ID = 0x082F; + static const uint16_t GPS_SPEED_ID = 0x083F; + static const uint16_t GPS_COURS_ID = 0x084F; static const uint16_t DIY_FIRST_ID = 0x5000; static const uint8_t FRAME_HEAD = 0x7E; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp index 3a1c7a3daa..084db99645 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp @@ -52,34 +52,31 @@ void AP_Frsky_SPort::send(void) case SENSOR_ID_VARIO: // Sensor ID 0 switch (_SPort.vario_call) { case 0: - send_sport_frame(SPORT_DATA_FRAME, DATA_ID_BARO_ALT_BP, _SPort_data.alt_nav_meters); // send altitude integer part + send_sport_frame(SPORT_DATA_FRAME, ALT_ID, _SPort_data.alt_nav_meters*100 + _SPort_data.alt_nav_cm); // send altitude in cm break; case 1: - send_sport_frame(SPORT_DATA_FRAME, DATA_ID_BARO_ALT_AP, _SPort_data.alt_nav_cm); // send altitude decimal part - break; - case 2: - send_sport_frame(SPORT_DATA_FRAME, DATA_ID_VARIO, _SPort_data.vario_vspd); // send vspeed m/s + send_sport_frame(SPORT_DATA_FRAME, VARIO_ID, _SPort_data.vario_vspd); // send vspeed cm/s _SPort.vario_refresh = true; break; } - if (++_SPort.vario_call > 2) { + if (++_SPort.vario_call > 1) { _SPort.vario_call = 0; } break; case SENSOR_ID_FAS: // Sensor ID 2 switch (_SPort.fas_call) { case 0: - send_sport_frame(SPORT_DATA_FRAME, DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining + send_sport_frame(SPORT_DATA_FRAME, FUEL_ID, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining as % break; case 1: - send_sport_frame(SPORT_DATA_FRAME, DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage + send_sport_frame(SPORT_DATA_FRAME, VFAS_ID, (uint16_t)roundf(_battery.voltage() * 100.0f)); // send battery voltage in cV break; case 2: { float current; if (!_battery.current_amps(current)) { current = 0; } - send_sport_frame(SPORT_DATA_FRAME, DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption + send_sport_frame(SPORT_DATA_FRAME, CURR_ID, (uint16_t)roundf(current * 10.0f)); // send current consumption in dA break; } break; @@ -97,23 +94,17 @@ void AP_Frsky_SPort::send(void) send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(_passthrough.send_latitude)); // gps latitude or longitude break; case 2: - send_sport_frame(SPORT_DATA_FRAME, DATA_ID_GPS_SPEED_BP, _SPort_data.speed_in_meter); // send gps speed integer part + send_sport_frame(SPORT_DATA_FRAME, GPS_SPEED_ID, _SPort_data.speed_in_meter*1000 + _SPort_data.speed_in_centimeter*10); // send gps speed in mm/sec break; case 3: - send_sport_frame(SPORT_DATA_FRAME, DATA_ID_GPS_SPEED_AP, _SPort_data.speed_in_centimeter); // send gps speed decimal part + send_sport_frame(SPORT_DATA_FRAME, GPS_ALT_ID, _SPort_data.alt_gps_meters*100+_SPort_data.alt_gps_cm); // send gps altitude in cm break; case 4: - send_sport_frame(SPORT_DATA_FRAME, DATA_ID_GPS_ALT_BP, _SPort_data.alt_gps_meters); // send gps altitude integer part - break; - case 5: - send_sport_frame(SPORT_DATA_FRAME, DATA_ID_GPS_ALT_AP, _SPort_data.alt_gps_cm); // send gps altitude decimals - break; - case 6: - send_sport_frame(SPORT_DATA_FRAME, DATA_ID_GPS_COURS_BP, _SPort_data.yaw); // send heading in degree based on AHRS and not GPS + send_sport_frame(SPORT_DATA_FRAME, GPS_COURS_ID, _SPort_data.yaw*100); // send heading in cd based on AHRS and not GPS _SPort.gps_refresh = true; break; } - if (++_SPort.gps_call > 6) { + if (++_SPort.gps_call > 4) { _SPort.gps_call = 0; } break; @@ -141,10 +132,10 @@ void AP_Frsky_SPort::send(void) case SENSOR_ID_SP2UR: // Sensor ID 6 switch (_SPort.various_call) { case 0 : - send_sport_frame(SPORT_DATA_FRAME, DATA_ID_TEMP2, (uint16_t)(AP::gps().num_sats() * 10 + AP::gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t) + send_sport_frame(SPORT_DATA_FRAME, TEMP2_ID, (uint16_t)(AP::gps().num_sats() * 10 + AP::gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t) break; case 1: - send_sport_frame(SPORT_DATA_FRAME, DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode + send_sport_frame(SPORT_DATA_FRAME, TEMP1_ID, gcs().custom_mode()); // send flight mode break; } if (++_SPort.various_call > 1) {