diff --git a/docs/Settings.md b/docs/Settings.md index 7cbba17cd8a..c42ac76ecc6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1082,39 +1082,9 @@ _// TODO_ --- -### frsky_coordinates_format - -D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA - -| Default | Min | Max | -| --- | --- | --- | -| 0 | 0 | FRSKY_FORMAT_NMEA | - ---- - -### frsky_default_latitude - -D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. - -| Default | Min | Max | -| --- | --- | --- | -| 0 | -90 | 90 | - ---- - -### frsky_default_longitude - -D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. - -| Default | Min | Max | -| --- | --- | --- | -| 0 | -180 | 180 | - ---- - ### frsky_pitch_roll -S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data +S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data | Default | Min | Max | | --- | --- | --- | @@ -1122,26 +1092,6 @@ S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw acc --- -### frsky_unit - -Not used? [METRIC/IMPERIAL] - -| Default | Min | Max | -| --- | --- | --- | -| METRIC | | | - ---- - -### frsky_vfas_precision - -D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method - -| Default | Min | Max | -| --- | --- | --- | -| 0 | FRSKY_VFAS_PRECISION_LOW | FRSKY_VFAS_PRECISION_HIGH | - ---- - ### fw_autotune_max_rate_deflection The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`. @@ -5054,7 +5004,7 @@ Selection of receiver (RX) type. Additional configuration of a `serialrx_provide ### report_cell_voltage -S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON +S.Port and IBUS telemetry: Send the average cell voltage if set to ON | Default | Min | Max | | --- | --- | --- | diff --git a/docs/Telemetry.md b/docs/Telemetry.md index b4c64ba1d2d..996630e66fa 100644 --- a/docs/Telemetry.md +++ b/docs/Telemetry.md @@ -125,40 +125,6 @@ The following sensors are transmitted To quickly and easily monitor these SmartPort sensors and flight modes, install [OpenTX Telemetry Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget) to your Taranis Q X7, X9D, X9D+ or X9E transmitter. -## FrSky telemetry - -FrSky telemetry is for older FrSky transmitters and D-series receivers. For newer transmitters paired with X-series receivers see SmartPort (S.Port) telemetry above. - -FrSky telemetry is transmit only and just requires a single connection from the TX pin of a serial port to the RX pin on an FrSky telemetry receiver. - -FrSky telemetry signals are inverted. To connect a INAV capable board to an FrSKy receiver you have some options. - -1. A hardware inverter - Built in to some flight controllers. -2. Use software serial. -3. Use a flight controller that has software configurable hardware inversion (e.g. F3 or F7). - -For 1, just connect your inverter to a usart or software serial port. - -For 2 and 3 use the CLI command as follows: - -``` -set telemetry_inverted = OFF -``` - -### Precision setting for VFAS - -INAV can send VFAS (FrSky Ampere Sensor Voltage) in two ways: - -``` -set frsky_vfas_precision = 0 -``` -This is default setting which supports VFAS resolution of 0.2 volts and is supported on all FrSky hardware. - -``` -set frsky_vfas_precision = 1 -``` -This is new setting which supports VFAS resolution of 0.1 volts and is supported by OpenTX and er9x/ersky9x firmware (this method uses custom ID 0x39). - ### Notes diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 317da760b6e..36bd62bf0bd 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -549,10 +549,6 @@ main_sources(COMMON_SRC telemetry/crsf.h telemetry/srxl.c telemetry/srxl.h - telemetry/frsky.c - telemetry/frsky.h - telemetry/frsky_d.c - telemetry/frsky_d.h telemetry/ghst.c telemetry/ghst.h telemetry/hott.c diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index e1633983b02..658f3799917 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -120,7 +120,6 @@ bool cliMode = false; #include "sensors/esc_sensor.h" #endif -#include "telemetry/frsky_d.h" #include "telemetry/telemetry.h" #include "build/debug.h" diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index b7fafafdd0f..f8ef157372f 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -489,7 +489,7 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn) return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT; } -#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) +#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) void releaseSharedTelemetryPorts(void) { serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1c0613893ea..e22e71fdf01 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -80,9 +80,6 @@ tables: - name: osd_alignment values: ["LEFT", "RIGHT"] enum: osd_alignment_e - - name: frsky_unit - values: ["METRIC", "IMPERIAL"] - enum: frskyUnit_e - name: ltm_rates values: ["NORMAL", "MEDIUM", "SLOW"] - name: i2c_speed @@ -2815,7 +2812,7 @@ groups: - name: PG_TELEMETRY_CONFIG type: telemetryConfig_t - headers: ["io/serial.h", "telemetry/telemetry.h", "telemetry/frsky_d.h", "telemetry/sim.h"] + headers: ["io/serial.h", "telemetry/telemetry.h", "telemetry/sim.h"] condition: USE_TELEMETRY members: - name: telemetry_switch @@ -2826,41 +2823,12 @@ groups: description: "Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used." default_value: OFF type: bool - - name: frsky_default_latitude - description: "D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired." - default_value: 0 - field: gpsNoFixLatitude - min: -90 - max: 90 - - name: frsky_default_longitude - description: "D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired." - default_value: 0 - field: gpsNoFixLongitude - min: -180 - max: 180 - - name: frsky_coordinates_format - description: "D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA" - default_value: 0 - field: frsky_coordinate_format - min: 0 - max: FRSKY_FORMAT_NMEA - type: uint8_t # TODO: This seems to use an enum, we should use table: - - name: frsky_unit - description: "Not used? [METRIC/IMPERIAL]" - default_value: "METRIC" - table: frsky_unit - type: uint8_t - - name: frsky_vfas_precision - description: "D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method" - default_value: 0 - min: FRSKY_VFAS_PRECISION_LOW - max: FRSKY_VFAS_PRECISION_HIGH - name: frsky_pitch_roll - description: "S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data" + description: "S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data" default_value: OFF type: bool - name: report_cell_voltage - description: "S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON" + description: "S.Port and IBUS telemetry: Send the average cell voltage if set to ON" default_value: OFF type: bool - name: hott_alarm_sound_interval diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 3ca560cf309..ffc85097b40 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -263,7 +263,7 @@ serialPort_t *findNextSharedSerialPort(uint32_t functionMask, serialPortFunction return NULL; } -#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) +#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) #define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK | FUNCTION_LOG) bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck) diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 4f727fd1001..9cd0e7e1d7b 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -33,7 +33,7 @@ typedef enum { FUNCTION_NONE = 0, FUNCTION_MSP = (1 << 0), // 1 FUNCTION_GPS = (1 << 1), // 2 - FUNCTION_TELEMETRY_FRSKY = (1 << 2), // 4 + FUNCTION_UNUSED_3 = (1 << 2), // 4 //Was FUNCTION_TELEMETRY_FRSKY FUNCTION_TELEMETRY_HOTT = (1 << 3), // 8 FUNCTION_TELEMETRY_LTM = (1 << 4), // 16 FUNCTION_TELEMETRY_SMARTPORT = (1 << 5), // 32 diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index aae496c07dd..6c123d0bfb6 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -73,7 +73,6 @@ void targetConfiguration(void) rxConfigMutable()->spektrum_sat_bind_autoreset = 1; } else { rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; - serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_TELEMETRY_FRSKY; featureConfigMutable()->enabledFeatures |= (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY); } diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index 0b56c008f5f..28d0465eaf7 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -75,7 +75,6 @@ void targetConfiguration(void) rxConfigMutable()->spektrum_sat_bind_autoreset = 1; } else { rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; - serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_TELEMETRY_FRSKY; featureConfigMutable()->enabledFeatures |= (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY); } diff --git a/src/main/target/common.h b/src/main/target/common.h index 2365be96712..f7ec27ccc03 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -58,7 +58,6 @@ #define USE_GPS_PROTO_MSP #define USE_TELEMETRY #define USE_TELEMETRY_LTM -#define USE_TELEMETRY_FRSKY #if defined(STM_FAST_TARGET) #define SCHEDULER_DELAY_LIMIT 10 diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c deleted file mode 100644 index 0896f412de6..00000000000 --- a/src/main/telemetry/frsky.c +++ /dev/null @@ -1,89 +0,0 @@ -/* - * frsky.c - * Common functions for FrSky D-series and SmartPort telemetry - */ - -#include -#include - -#include "platform.h" - -#if defined(USE_TELEMETRY) && (defined(USE_TELEMETRY_FRSKY) || defined(USE_TELEMETRY_SMARTPORT)) - -#include "common/maths.h" -#include "fc/runtime_config.h" -#include "fc/rc_modes.h" -#include "io/gps.h" -#include "telemetry/frsky.h" - -uint16_t frskyGetFlightMode(void) -{ - uint16_t tmpi = 0; - - // ones column - if (!isArmingDisabled()) - tmpi += 1; - else - tmpi += 2; - if (ARMING_FLAG(ARMED)) - tmpi += 4; - - // tens column - if (FLIGHT_MODE(ANGLE_MODE)) - tmpi += 10; - if (FLIGHT_MODE(HORIZON_MODE)) - tmpi += 20; - if (FLIGHT_MODE(MANUAL_MODE)) - tmpi += 40; - - // hundreds column - if (FLIGHT_MODE(HEADING_MODE)) - tmpi += 100; - if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) - tmpi += 200; - if (FLIGHT_MODE(NAV_POSHOLD_MODE)) - tmpi += 400; - - // thousands column - if (FLIGHT_MODE(NAV_RTH_MODE)) - tmpi += 1000; - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) // intentionally out of order and 'else-ifs' to prevent column overflow - tmpi += 8000; - else if (FLIGHT_MODE(NAV_WP_MODE)) - tmpi += 2000; - else if (FLIGHT_MODE(HEADFREE_MODE)) - tmpi += 4000; - - // ten thousands column - if (FLIGHT_MODE(FLAPERON)) - tmpi += 10000; - if (FLIGHT_MODE(FAILSAFE_MODE)) - tmpi += 40000; - else if (FLIGHT_MODE(AUTO_TUNE)) // intentionally reverse order and 'else-if' to prevent 16-bit overflow - tmpi += 20000; - - return tmpi; -} - -uint16_t frskyGetGPSState(void) -{ - uint16_t tmpi = 0; - - // ones and tens columns (# of satellites 0 - 99) - tmpi += constrain(gpsSol.numSat, 0, 99); - - // hundreds column (satellite accuracy HDOP: 0 = worst [HDOP > 5.5], 9 = best [HDOP <= 1.0]) - tmpi += (9 - constrain((gpsSol.hdop - 51) / 50, 0, 9)) * 100; - - // thousands column (GPS fix status) - if (STATE(GPS_FIX)) - tmpi += 1000; - if (STATE(GPS_FIX_HOME)) - tmpi += 2000; - if (ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) - tmpi += 4000; - - return tmpi; -} - -#endif diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h deleted file mode 100644 index 9d0e71b4823..00000000000 --- a/src/main/telemetry/frsky.h +++ /dev/null @@ -1,26 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * INAV is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with INAV. If not, see . - */ - -/* - * frsky.h - * Common functions for FrSky D-series and SmartPort telemetry - */ - -#pragma once - -uint16_t frskyGetFlightMode(void); -uint16_t frskyGetGPSState(void); diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c deleted file mode 100644 index 3ba09eba381..00000000000 --- a/src/main/telemetry/frsky_d.c +++ /dev/null @@ -1,567 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -/* - * Initial FrSky Telemetry implementation by silpstream @ rcgroups. - * Addition protocol work by airmamaf @ github. - */ -#include -#include -#include -#include - -#include "platform.h" - -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_FRSKY) - -#include "common/maths.h" -#include "common/axis.h" - -#include "config/feature.h" - -#include "drivers/time.h" -#include "drivers/serial.h" - -#include "fc/rc_controls.h" -#include "fc/runtime_config.h" - -#include "fc/config.h" -#include "fc/rc_modes.h" - -#include "flight/mixer.h" -#include "flight/pid.h" -#include "flight/imu.h" - -#include "io/gps.h" -#include "io/serial.h" - -#include "navigation/navigation.h" - -#include "sensors/acceleration.h" -#include "sensors/barometer.h" -#include "sensors/battery.h" -#include "sensors/gyro.h" -#include "sensors/sensors.h" - -#include "telemetry/telemetry.h" -#include "telemetry/frsky_d.h" -#include "telemetry/frsky.h" - -static serialPort_t *frskyPort = NULL; -static serialPortConfig_t *portConfig; - -#define FRSKY_BAUDRATE 9600 -#define FRSKY_INITIAL_PORT_MODE MODE_TX - -static bool frskyTelemetryEnabled = false; -static portSharing_e frskyPortSharing; - -#define CYCLETIME 125 - -#define PROTOCOL_HEADER 0x5E -#define PROTOCOL_TAIL 0x5E - -// Data Ids (bp = before decimal point; af = after decimal point) -// Official data IDs -#define ID_GPS_ALTIDUTE_BP 0x01 -#define ID_GPS_ALTIDUTE_AP 0x09 -#define ID_TEMPRATURE1 0x02 -#define ID_RPM 0x03 -#define ID_FUEL_LEVEL 0x04 -#define ID_TEMPRATURE2 0x05 -#define ID_VOLT 0x06 -#define ID_ALTITUDE_BP 0x10 -#define ID_ALTITUDE_AP 0x21 -#define ID_GPS_SPEED_BP 0x11 -#define ID_GPS_SPEED_AP 0x19 -#define ID_LONGITUDE_BP 0x12 -#define ID_LONGITUDE_AP 0x1A -#define ID_E_W 0x22 -#define ID_LATITUDE_BP 0x13 -#define ID_LATITUDE_AP 0x1B -#define ID_N_S 0x23 -#define ID_COURSE_BP 0x14 -#define ID_COURSE_AP 0x1C -#define ID_DATE_MONTH 0x15 -#define ID_YEAR 0x16 -#define ID_HOUR_MINUTE 0x17 -#define ID_SECOND 0x18 -#define ID_ACC_X 0x24 -#define ID_ACC_Y 0x25 -#define ID_ACC_Z 0x26 -#define ID_VOLTAGE_AMP 0x39 -#define ID_VOLTAGE_AMP_BP 0x3A -#define ID_VOLTAGE_AMP_AP 0x3B -#define ID_CURRENT 0x28 -// User defined data IDs -#define ID_GYRO_X 0x40 -#define ID_GYRO_Y 0x41 -#define ID_GYRO_Z 0x42 -#define ID_HOME_DIST 0x07 -#define ID_PITCH 0x08 -#define ID_ROLL 0x20 - -#define ID_VERT_SPEED 0x30 //opentx vario - -#define GPS_BAD_QUALITY 300 -#define GPS_MAX_HDOP_VAL 9999 -#define DELAY_FOR_BARO_INITIALISATION (5 * 1000) //5s -#define BLADE_NUMBER_DIVIDER 5 // should set 12 blades in Taranis - -static uint32_t lastCycleTime = 0; -static uint8_t cycleNum = 0; -static void sendDataHead(uint8_t id) -{ - serialWrite(frskyPort, PROTOCOL_HEADER); - serialWrite(frskyPort, id); -} - -static void sendTelemetryTail(void) -{ - serialWrite(frskyPort, PROTOCOL_TAIL); -} - -static void serializeFrsky(uint8_t data) -{ - // take care of byte stuffing - if (data == 0x5e) { - serialWrite(frskyPort, 0x5d); - serialWrite(frskyPort, 0x3e); - } else if (data == 0x5d) { - serialWrite(frskyPort, 0x5d); - serialWrite(frskyPort, 0x3d); - } else - serialWrite(frskyPort, data); -} - -static void serialize16(int16_t a) -{ - uint8_t t; - t = a; - serializeFrsky(t); - t = a >> 8 & 0xff; - serializeFrsky(t); -} - -static void sendAccel(void) -{ - int i; - - for (i = 0; i < 3; i++) { - sendDataHead(ID_ACC_X + i); - serialize16(lrintf(acc.accADCf[i] * 1000)); - } -} - -static void sendBaro(void) -{ - const int32_t alt = getEstimatedActualPosition(Z); - sendDataHead(ID_ALTITUDE_BP); - serialize16(alt / 100); - sendDataHead(ID_ALTITUDE_AP); - serialize16(ABS(alt % 100)); -} - -#ifdef USE_GPS -static void sendGpsAltitude(void) -{ - uint16_t altitude = gpsSol.llh.alt / 100; // meters - //Send real GPS altitude only if it's reliable (there's a GPS fix) - if (!STATE(GPS_FIX)) { - altitude = 0; - } - sendDataHead(ID_GPS_ALTIDUTE_BP); - serialize16(altitude); - sendDataHead(ID_GPS_ALTIDUTE_AP); - serialize16(0); -} -#endif - -static void sendThrottleOrBatterySizeAsRpm(void) -{ - sendDataHead(ID_RPM); - if (ARMING_FLAG(ARMED)) { - uint16_t throttleForRPM = getThrottlePercent() / BLADE_NUMBER_DIVIDER; - serialize16(throttleForRPM); - } else { - serialize16((currentBatteryProfile->capacity.value / BLADE_NUMBER_DIVIDER)); - } - -} - -static void sendFlightModeAsTemperature1(void) -{ - sendDataHead(ID_TEMPRATURE1); - serialize16(frskyGetFlightMode()); -} - -#ifdef USE_GPS -static void sendSatalliteSignalQualityAsTemperature2(void) -{ - sendDataHead(ID_TEMPRATURE2); - serialize16(frskyGetGPSState()); -} - -static void sendSpeed(void) -{ - if (!STATE(GPS_FIX)) { - return; - } - //Speed should be sent in knots (GPS speed is in cm/s) - sendDataHead(ID_GPS_SPEED_BP); - //convert to knots: 1cm/s = 0.0194384449 knots - serialize16(gpsSol.groundSpeed * 1944 / 100000); - sendDataHead(ID_GPS_SPEED_AP); - serialize16((gpsSol.groundSpeed * 1944 / 100) % 100); -} - -static void sendHomeDistance(void) -{ - if (!STATE(GPS_FIX)) { - return; - } - sendDataHead(ID_HOME_DIST); - serialize16(GPS_distanceToHome); -} - -#endif - -static void sendTime(void) -{ - uint32_t seconds = millis() / 1000; - uint8_t minutes = (seconds / 60) % 60; - - // if we fly for more than an hour, something's wrong anyway - sendDataHead(ID_HOUR_MINUTE); - serialize16(minutes << 8); - sendDataHead(ID_SECOND); - serialize16(seconds % 60); -} - -// Frsky pdf: dddmm.mmmm -// .mmmm is returned in decimal fraction of minutes. -static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) -{ - int32_t absgps, deg, min; - absgps = ABS(mwiigps); - deg = absgps / GPS_DEGREES_DIVIDER; - absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7 - min = absgps / GPS_DEGREES_DIVIDER; // minutes left - - result->dddmm = deg * ((FRSKY_FORMAT_DMS == telemetryConfig()->frsky_coordinate_format) ? (100) : (60)) + min; - - result->mmmm = (absgps - min * GPS_DEGREES_DIVIDER) / 1000; -} - -static void sendLatLong(int32_t coord[2]) -{ - gpsCoordinateDDDMMmmmm_t coordinate; - GPStoDDDMM_MMMM(coord[LAT], &coordinate); - sendDataHead(ID_LATITUDE_BP); - serialize16(coordinate.dddmm); - sendDataHead(ID_LATITUDE_AP); - serialize16(coordinate.mmmm); - sendDataHead(ID_N_S); - serialize16(coord[LAT] < 0 ? 'S' : 'N'); - - GPStoDDDMM_MMMM(coord[LON], &coordinate); - sendDataHead(ID_LONGITUDE_BP); - serialize16(coordinate.dddmm); - sendDataHead(ID_LONGITUDE_AP); - serialize16(coordinate.mmmm); - sendDataHead(ID_E_W); - serialize16(coord[LON] < 0 ? 'W' : 'E'); -} - -#ifdef USE_GPS -static void sendFakeLatLong(void) -{ - // Heading is only displayed on OpenTX if non-zero lat/long is also sent - int32_t coord[2] = {0,0}; - - coord[LAT] = (telemetryConfig()->gpsNoFixLatitude * GPS_DEGREES_DIVIDER); - coord[LON] = (telemetryConfig()->gpsNoFixLongitude * GPS_DEGREES_DIVIDER); - - sendLatLong(coord); -} -#endif - -static void sendFakeLatLongThatAllowsHeadingDisplay(void) -{ - // Heading is only displayed on OpenTX if non-zero lat/long is also sent - int32_t coord[2] = { - 1 * GPS_DEGREES_DIVIDER, - 1 * GPS_DEGREES_DIVIDER - }; - - sendLatLong(coord); -} - -#ifdef USE_GPS -static void sendGPSLatLong(void) -{ - static uint8_t gpsFixOccured = 0; - int32_t coord[2] = {0,0}; - - if (STATE(GPS_FIX) || gpsFixOccured == 1) { - // If we have ever had a fix, send the last known lat/long - gpsFixOccured = 1; - coord[LAT] = gpsSol.llh.lat; - coord[LON] = gpsSol.llh.lon; - sendLatLong(coord); - } else { - // otherwise send fake lat/long in order to display compass value - sendFakeLatLong(); - } -} -#endif - -/* - * Send vertical speed for opentx. ID_VERT_SPEED - * Unit is cm/s - */ -static void sendVario(void) -{ - sendDataHead(ID_VERT_SPEED); - serialize16((int16_t)lrintf(getEstimatedActualVelocity(Z))); -} - -/* - * Send voltage via ID_VOLT - * - * NOTE: This sends voltage divided by batteryCellCount. To get the real - * battery voltage, you need to multiply the value by batteryCellCount. - */ -static void sendVoltage(void) -{ - uint32_t cellVoltage; - uint16_t payload; - - /* - * Format for Voltage Data for single cells is like this: - * - * llll llll cccc hhhh - * l: Low voltage bits - * h: High voltage bits - * c: Cell number (starting at 0) - * - * The actual value sent for cell voltage has resolution of 0.002 volts - * Since vbat has resolution of 0.01 volts it has to be multiplied by 5 - */ - cellVoltage = ((uint32_t)getBatteryVoltage() * 10) / (getBatteryCellCount() * 2); - - // Cell number is at bit 9-12 (only uses vbat, so it can't send individual cell voltages, set cell number to 0) - payload = 0; - - // Lower voltage bits are at bit 0-8 - payload |= ((cellVoltage & 0x0ff) << 8); - - // Higher voltage bits are at bits 13-15 - payload |= ((cellVoltage & 0xf00) >> 8); - - sendDataHead(ID_VOLT); - serialize16(payload); -} - -/* - * Send voltage with ID_VOLTAGE_AMP - */ -static void sendVoltageAmp(void) -{ - uint16_t vbat = getBatteryVoltage(); - if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) { - /* - * Use new ID 0x39 to send voltage directly in 0.1 volts resolution - */ - sendDataHead(ID_VOLTAGE_AMP); - serialize16(vbat / 10); - } else { - uint16_t voltage = (vbat * 11) / 21; - uint16_t vfasVoltage; - if (telemetryConfig()->report_cell_voltage) { - vfasVoltage = voltage / getBatteryCellCount(); - } else { - vfasVoltage = voltage; - } - sendDataHead(ID_VOLTAGE_AMP_BP); - serialize16(vfasVoltage / 100); - sendDataHead(ID_VOLTAGE_AMP_AP); - serialize16(((vfasVoltage % 100) + 5) / 10); - } -} - -static void sendAmperage(void) -{ - sendDataHead(ID_CURRENT); - serialize16((uint16_t)(getAmperage() / 10)); -} - -static void sendFuelLevel(void) -{ - if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) { - sendDataHead(ID_FUEL_LEVEL); - serialize16((uint16_t)calculateBatteryPercentage()); - } else if (isAmperageConfigured()) { - sendDataHead(ID_FUEL_LEVEL); - serialize16((uint16_t)telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_MAH ? getMAhDrawn() : getMWhDrawn()); - } -} - -static void sendHeading(void) -{ - sendDataHead(ID_COURSE_BP); - serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); - sendDataHead(ID_COURSE_AP); - serialize16(0); -} - -static void sendPitch(void) -{ - sendDataHead(ID_PITCH); - serialize16(attitude.values.pitch); -} - -static void sendRoll(void) -{ - sendDataHead(ID_ROLL); - serialize16(attitude.values.roll); -} - -void initFrSkyTelemetry(void) -{ - portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY); - frskyPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY); -} - -void freeFrSkyTelemetryPort(void) -{ - closeSerialPort(frskyPort); - frskyPort = NULL; - frskyTelemetryEnabled = false; -} - -void configureFrSkyTelemetryPort(void) -{ - if (!portConfig) { - return; - } - - frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED); - if (!frskyPort) { - return; - } - - frskyTelemetryEnabled = true; -} - -bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis) -{ - return currentMillis - lastCycleTime >= CYCLETIME; -} - -void checkFrSkyTelemetryState(void) -{ - if (portConfig && telemetryCheckRxPortShared(portConfig)) { - if (!frskyTelemetryEnabled && telemetrySharedPort != NULL) { - frskyPort = telemetrySharedPort; - frskyTelemetryEnabled = true; - } - } else { - bool newTelemetryEnabledValue = telemetryDetermineEnabledState(frskyPortSharing); - - if (newTelemetryEnabledValue == frskyTelemetryEnabled) { - return; - } - - if (newTelemetryEnabledValue) - configureFrSkyTelemetryPort(); - else - freeFrSkyTelemetryPort(); - } -} - -void handleFrSkyTelemetry(void) -{ - if (!frskyTelemetryEnabled) { - return; - } - - uint32_t now = millis(); - - if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) { - return; - } - - lastCycleTime = now; - - cycleNum++; - - // Sent every 125ms - if (telemetryConfig()->frsky_pitch_roll) { - sendPitch(); - sendRoll(); - } else { - sendAccel(); - } - sendVario(); - sendTelemetryTail(); - - if ((cycleNum % 4) == 0) { // Sent every 500ms - if (lastCycleTime > DELAY_FOR_BARO_INITIALISATION) { //Allow 5s to boot correctly - sendBaro(); - } - sendHeading(); - sendTelemetryTail(); - } - - if ((cycleNum % 8) == 0) { // Sent every 1s - sendFlightModeAsTemperature1(); - sendThrottleOrBatterySizeAsRpm(); - - if (feature(FEATURE_VBAT)) { - sendVoltage(); - sendVoltageAmp(); - sendAmperage(); - sendFuelLevel(); - } - -#ifdef USE_GPS - if (sensors(SENSOR_GPS)) { - sendSpeed(); - sendHomeDistance(); - sendGpsAltitude(); - sendSatalliteSignalQualityAsTemperature2(); - sendGPSLatLong(); - } - else { - sendFakeLatLongThatAllowsHeadingDisplay(); - } -#else - sendFakeLatLongThatAllowsHeadingDisplay(); -#endif - - sendTelemetryTail(); - } - - if (cycleNum == 40) { //Frame 3: Sent every 5s - cycleNum = 0; - sendTime(); - sendTelemetryTail(); - } -} - -#endif diff --git a/src/main/telemetry/frsky_d.h b/src/main/telemetry/frsky_d.h deleted file mode 100644 index fe4846d0114..00000000000 --- a/src/main/telemetry/frsky_d.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -typedef enum { - FRSKY_VFAS_PRECISION_LOW = 0, - FRSKY_VFAS_PRECISION_HIGH -} frskyVFasPrecision_e; - -void handleFrSkyTelemetry(void); -void checkFrSkyTelemetryState(void); - -void initFrSkyTelemetry(void); -void configureFrSkyTelemetryPort(void); -void freeFrSkyTelemetryPort(void); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 1ea659864bd..81d5f793097 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -59,7 +59,6 @@ FILE_COMPILE_FOR_SPEED #include "telemetry/telemetry.h" #include "telemetry/smartport.h" -#include "telemetry/frsky.h" #include "telemetry/msp_shared.h" // these data identifiers are obtained from https://github.com/opentx/opentx/blob/2.3/radio/src/telemetry/frsky.h @@ -162,6 +161,76 @@ static smartPortWriteFrameFn *smartPortWriteFrame; static bool smartPortMspReplyPending = false; #endif +static uint16_t frskyGetFlightMode(void) +{ + uint16_t tmpi = 0; + + // ones column + if (!isArmingDisabled()) + tmpi += 1; + else + tmpi += 2; + if (ARMING_FLAG(ARMED)) + tmpi += 4; + + // tens column + if (FLIGHT_MODE(ANGLE_MODE)) + tmpi += 10; + if (FLIGHT_MODE(HORIZON_MODE)) + tmpi += 20; + if (FLIGHT_MODE(MANUAL_MODE)) + tmpi += 40; + + // hundreds column + if (FLIGHT_MODE(HEADING_MODE)) + tmpi += 100; + if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) + tmpi += 200; + if (FLIGHT_MODE(NAV_POSHOLD_MODE)) + tmpi += 400; + + // thousands column + if (FLIGHT_MODE(NAV_RTH_MODE)) + tmpi += 1000; + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) // intentionally out of order and 'else-ifs' to prevent column overflow + tmpi += 8000; + else if (FLIGHT_MODE(NAV_WP_MODE)) + tmpi += 2000; + else if (FLIGHT_MODE(HEADFREE_MODE)) + tmpi += 4000; + + // ten thousands column + if (FLIGHT_MODE(FLAPERON)) + tmpi += 10000; + if (FLIGHT_MODE(FAILSAFE_MODE)) + tmpi += 40000; + else if (FLIGHT_MODE(AUTO_TUNE)) // intentionally reverse order and 'else-if' to prevent 16-bit overflow + tmpi += 20000; + + return tmpi; +} + +static uint16_t frskyGetGPSState(void) +{ + uint16_t tmpi = 0; + + // ones and tens columns (# of satellites 0 - 99) + tmpi += constrain(gpsSol.numSat, 0, 99); + + // hundreds column (satellite accuracy HDOP: 0 = worst [HDOP > 5.5], 9 = best [HDOP <= 1.0]) + tmpi += (9 - constrain((gpsSol.hdop - 51) / 50, 0, 9)) * 100; + + // thousands column (GPS fix status) + if (STATE(GPS_FIX)) + tmpi += 1000; + if (STATE(GPS_FIX_HOME)) + tmpi += 2000; + if (ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) + tmpi += 4000; + + return tmpi; +} + smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool useChecksum) { static uint8_t rxBuffer[sizeof(smartPortPayload_t)]; diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 00a5f182ae0..0ed26e250fc 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -41,7 +41,6 @@ #include "rx/rx.h" #include "telemetry/telemetry.h" -#include "telemetry/frsky_d.h" #include "telemetry/hott.h" #include "telemetry/smartport.h" #include "telemetry/ltm.h" @@ -54,16 +53,11 @@ #include "telemetry/ghst.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 5); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 6); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, - .gpsNoFixLatitude = SETTING_FRSKY_DEFAULT_LATITUDE_DEFAULT, - .gpsNoFixLongitude = SETTING_FRSKY_DEFAULT_LONGITUDE_DEFAULT, .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT, .telemetry_inverted = SETTING_TELEMETRY_INVERTED_DEFAULT, - .frsky_coordinate_format = SETTING_FRSKY_COORDINATES_FORMAT_DEFAULT, - .frsky_unit = SETTING_FRSKY_UNIT_DEFAULT, - .frsky_vfas_precision = SETTING_FRSKY_VFAS_PRECISION_DEFAULT, .frsky_pitch_roll = SETTING_FRSKY_PITCH_ROLL_DEFAULT, .report_cell_voltage = SETTING_REPORT_CELL_VOLTAGE_DEFAULT, .hottAlarmSoundInterval = SETTING_HOTT_ALARM_SOUND_INTERVAL_DEFAULT, @@ -97,9 +91,6 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, void telemetryInit(void) { -#if defined(USE_TELEMETRY_FRSKY) - initFrSkyTelemetry(); -#endif #if defined(USE_TELEMETRY_HOTT) initHoTTTelemetry(); @@ -167,9 +158,6 @@ serialPort_t *telemetrySharedPort = NULL; void telemetryCheckState(void) { -#if defined(USE_TELEMETRY_FRSKY) - checkFrSkyTelemetryState(); -#endif #if defined(USE_TELEMETRY_HOTT) checkHoTTTelemetryState(); @@ -215,10 +203,6 @@ void telemetryProcess(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); // since not used by all the telemetry protocols -#if defined(USE_TELEMETRY_FRSKY) - handleFrSkyTelemetry(); -#endif - #if defined(USE_TELEMETRY_HOTT) handleHoTTTelemetry(currentTimeUs); #endif diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 51e79b89b5e..e6146a03938 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -36,11 +36,6 @@ typedef enum { FRSKY_FORMAT_NMEA } frskyGpsCoordFormat_e; -typedef enum { - FRSKY_UNIT_METRICS = 0, - FRSKY_UNIT_IMPERIALS -} frskyUnit_e; - typedef enum { LTM_RATE_NORMAL, LTM_RATE_MEDIUM, @@ -54,13 +49,8 @@ typedef enum { } smartportFuelUnit_e; typedef struct telemetryConfig_s { - float gpsNoFixLatitude; - float gpsNoFixLongitude; uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. uint8_t telemetry_inverted; // Flip the default inversion of the protocol - Same as serialrx_inverted in rx.c, but for telemetry. - frskyGpsCoordFormat_e frsky_coordinate_format; - frskyUnit_e frsky_unit; - uint8_t frsky_vfas_precision; uint8_t frsky_pitch_roll; uint8_t report_cell_voltage; uint8_t hottAlarmSoundInterval; @@ -93,7 +83,7 @@ typedef struct telemetryConfig_s { PG_DECLARE(telemetryConfig_t, telemetryConfig); -#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_IBUS) +#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_IBUS) extern serialPort_t *telemetrySharedPort; void telemetryInit(void); diff --git a/src/test/unit/target.h b/src/test/unit/target.h index 4e50761db89..b95e1678c78 100755 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -24,7 +24,6 @@ #define USE_GPS_PROTO_UBLOX #define USE_DASHBOARD #define USE_TELEMETRY -#define USE_TELEMETRY_FRSKY #define USE_TELEMETRY_HOTT #define USE_TELEMETRY_IBUS #define USE_TELEMETRY_SMARTPORT