From 66383c948fc15a1011736448ff5ea11bf273ef7c Mon Sep 17 00:00:00 2001 From: Alexey Stankevich Date: Tue, 24 Apr 2018 21:45:54 +0300 Subject: [PATCH 1/9] Modify FrSky D-series telemetry for compatibility with Lua script --- src/main/telemetry/frsky.c | 91 +++++++++++++++++++++++++++++++------- 1 file changed, 76 insertions(+), 15 deletions(-) diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index a608f7cf5fa..444559a8f98 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -40,6 +40,7 @@ #include "fc/runtime_config.h" #include "fc/config.h" +#include "fc/rc_modes.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -109,6 +110,7 @@ static portSharing_e frskyPortSharing; #define ID_GYRO_X 0x40 #define ID_GYRO_Y 0x41 #define ID_GYRO_Z 0x42 +#define ID_HOME_DIST 0x07 #define ID_VERT_SPEED 0x30 //opentx vario @@ -201,28 +203,76 @@ static void sendThrottleOrBatterySizeAsRpm(void) } -static void sendTemperature1(void) +static void sendFlightModeAsTemperature1(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_WP_MODE)) + tmpi += 2000; + if (FLIGHT_MODE(HEADFREE_MODE)) + tmpi += 4000; + + // ten thousands column + if (FLIGHT_MODE(FLAPERON)) + tmpi += 10000; + if (FLIGHT_MODE(AUTO_TUNE)) + tmpi += 20000; + if (FLIGHT_MODE(FAILSAFE_MODE)) + tmpi += 40000; + // todo: prevent 16-bit variable from overflow + sendDataHead(ID_TEMPRATURE1); -#ifdef USE_BARO - serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf -#else - /* - * There is no temperature information, so send 0 - */ - serialize16(0); -#endif + serialize16(tmpi); } #ifdef USE_GPS static void sendSatalliteSignalQualityAsTemperature2(void) { - uint16_t satellite = gpsSol.numSat; - if (gpsSol.hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s - satellite = constrain(gpsSol.hdop, 0, GPS_MAX_HDOP_VAL); - } + uint32_t tmpi = 0; + + // ones and tens columns (# of satellites 0 - 99) + tmpi += constrain(14/*gpsSol.numSat*/, 0, 99); + + // hundreds column (satellite accuracy HDOP: 0 = worst, 9 = best) + tmpi += (9 - constrain(/*gpsSol.hdop*/ 6000 / 1000, 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; + sendDataHead(ID_TEMPRATURE2); - serialize16(satellite); + serialize16(tmpi); } static void sendSpeed(void) @@ -237,6 +287,16 @@ static void sendSpeed(void) 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) @@ -511,7 +571,7 @@ void handleFrSkyTelemetry(void) } if ((cycleNum % 8) == 0) { // Sent every 1s - sendTemperature1(); + sendFlightModeAsTemperature1(); sendThrottleOrBatterySizeAsRpm(); if (feature(FEATURE_VBAT)) { @@ -524,6 +584,7 @@ void handleFrSkyTelemetry(void) #ifdef USE_GPS if (sensors(SENSOR_GPS)) { sendSpeed(); + sendHomeDistance(); sendGpsAltitude(); sendSatalliteSignalQualityAsTemperature2(); sendGPSLatLong(); From 3372082afffd37ddf89c1b7c4b42b1f724ceccf6 Mon Sep 17 00:00:00 2001 From: Alexey Stankevich Date: Sun, 29 Apr 2018 19:34:15 +0300 Subject: [PATCH 2/9] replace dummy values with real ones --- src/main/telemetry/frsky.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 444559a8f98..8bf7245cf33 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -258,10 +258,10 @@ static void sendSatalliteSignalQualityAsTemperature2(void) uint32_t tmpi = 0; // ones and tens columns (# of satellites 0 - 99) - tmpi += constrain(14/*gpsSol.numSat*/, 0, 99); + tmpi += constrain(gpsSol.numSat, 0, 99); // hundreds column (satellite accuracy HDOP: 0 = worst, 9 = best) - tmpi += (9 - constrain(/*gpsSol.hdop*/ 6000 / 1000, 0, 9)) * 100; + tmpi += (9 - constrain(gpsSol.hdop / 1000, 0, 9)) * 100; // thousands column (GPS fix status) if (STATE(GPS_FIX)) From cbdfa31c76573342a0f4f07226f6dad6bdb08c56 Mon Sep 17 00:00:00 2001 From: Alexey Stankevich Date: Sun, 6 May 2018 17:14:18 +0300 Subject: [PATCH 3/9] Rename 'frsky.c/h' to 'frsky_d.c/h' --- Makefile | 2 +- src/main/fc/cli.c | 2 +- src/main/fc/settings.yaml | 2 +- src/main/telemetry/{frsky.c => frsky_d.c} | 2 +- src/main/telemetry/{frsky.h => frsky_d.h} | 0 src/main/telemetry/telemetry.c | 2 +- 6 files changed, 5 insertions(+), 5 deletions(-) rename src/main/telemetry/{frsky.c => frsky_d.c} (99%) rename src/main/telemetry/{frsky.h => frsky_d.h} (100%) diff --git a/Makefile b/Makefile index 717e67c348a..6d01181a515 100644 --- a/Makefile +++ b/Makefile @@ -721,7 +721,7 @@ HIGHEND_SRC = \ sensors/rangefinder.c \ sensors/opflow.c \ telemetry/crsf.c \ - telemetry/frsky.c \ + telemetry/frsky_d.c \ telemetry/hott.c \ telemetry/ibus_shared.c \ telemetry/ibus.c \ diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index b4c68548cb2..c65d57b215c 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -113,7 +113,7 @@ extern uint8_t __config_end; #include "sensors/opflow.h" #include "sensors/sensors.h" -#include "telemetry/frsky.h" +#include "telemetry/frsky_d.h" #include "telemetry/telemetry.h" #include "build/debug.h" diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d81aa2226ba..950434d9809 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1303,7 +1303,7 @@ groups: - name: PG_TELEMETRY_CONFIG type: telemetryConfig_t - headers: ["io/serial.h", "telemetry/telemetry.h", "telemetry/frsky.h"] + headers: ["io/serial.h", "telemetry/telemetry.h", "telemetry/frsky_d.h"] condition: USE_TELEMETRY members: - name: telemetry_switch diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky_d.c similarity index 99% rename from src/main/telemetry/frsky.c rename to src/main/telemetry/frsky_d.c index 8bf7245cf33..2e1f4b51aac 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky_d.c @@ -58,7 +58,7 @@ #include "sensors/sensors.h" #include "telemetry/telemetry.h" -#include "telemetry/frsky.h" +#include "telemetry/frsky_d.h" static serialPort_t *frskyPort = NULL; static serialPortConfig_t *portConfig; diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky_d.h similarity index 100% rename from src/main/telemetry/frsky.h rename to src/main/telemetry/frsky_d.h diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 4a2053a438e..3beb31704eb 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -40,7 +40,7 @@ #include "rx/rx.h" #include "telemetry/telemetry.h" -#include "telemetry/frsky.h" +#include "telemetry/frsky_d.h" #include "telemetry/hott.h" #include "telemetry/smartport.h" #include "telemetry/ltm.h" From aa55e7d4d90b73df44c02f9f1915ddf88cc1bd64 Mon Sep 17 00:00:00 2001 From: Alexey Stankevich Date: Sun, 6 May 2018 21:56:40 +0300 Subject: [PATCH 4/9] Move common code for FrSky D-series and SmartPort telemetry to the separate file --- Makefile | 1 + src/main/telemetry/frsky.c | 89 ++++++++++++++++++++++++++++++++++ src/main/telemetry/frsky.h | 9 ++++ src/main/telemetry/frsky_d.c | 64 ++---------------------- src/main/telemetry/smartport.c | 64 ++---------------------- 5 files changed, 105 insertions(+), 122 deletions(-) create mode 100644 src/main/telemetry/frsky.c create mode 100644 src/main/telemetry/frsky.h diff --git a/Makefile b/Makefile index 6d01181a515..d46bec3f54c 100644 --- a/Makefile +++ b/Makefile @@ -722,6 +722,7 @@ HIGHEND_SRC = \ sensors/opflow.c \ telemetry/crsf.c \ telemetry/frsky_d.c \ + telemetry/frsky.c \ telemetry/hott.c \ telemetry/ibus_shared.c \ telemetry/ibus.c \ diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c new file mode 100644 index 00000000000..2c1000f94a9 --- /dev/null +++ b/src/main/telemetry/frsky.c @@ -0,0 +1,89 @@ +/* + * 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" + +uint32_t frskyGetFlightMode(void) +{ + uint32_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_WP_MODE)) + tmpi += 2000; + if (FLIGHT_MODE(HEADFREE_MODE)) + tmpi += 4000; + + // ten thousands column + if (FLIGHT_MODE(FLAPERON)) + tmpi += 10000; + if (FLIGHT_MODE(AUTO_TUNE)) + tmpi += 20000; + if (FLIGHT_MODE(FAILSAFE_MODE)) + tmpi += 40000; + // todo: prevent 16-bit variable from overflow, + // D-series receivers transmit only 16-bit values + + 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, 9 = best) + tmpi += (9 - constrain(gpsSol.hdop / 1000, 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 new file mode 100644 index 00000000000..3ead24cc559 --- /dev/null +++ b/src/main/telemetry/frsky.h @@ -0,0 +1,9 @@ +/* + * frsky.h + * Common functions for FrSky D-series and SmartPort telemetry + */ + +#pragma once + +uint32_t frskyGetFlightMode(void); +uint16_t frskyGetGPSState(void); diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c index 2e1f4b51aac..8cfde87182a 100644 --- a/src/main/telemetry/frsky_d.c +++ b/src/main/telemetry/frsky_d.c @@ -59,6 +59,7 @@ #include "telemetry/telemetry.h" #include "telemetry/frsky_d.h" +#include "telemetry/frsky.h" static serialPort_t *frskyPort = NULL; static serialPortConfig_t *portConfig; @@ -205,74 +206,15 @@ static void sendThrottleOrBatterySizeAsRpm(void) static void sendFlightModeAsTemperature1(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_WP_MODE)) - tmpi += 2000; - if (FLIGHT_MODE(HEADFREE_MODE)) - tmpi += 4000; - - // ten thousands column - if (FLIGHT_MODE(FLAPERON)) - tmpi += 10000; - if (FLIGHT_MODE(AUTO_TUNE)) - tmpi += 20000; - if (FLIGHT_MODE(FAILSAFE_MODE)) - tmpi += 40000; - // todo: prevent 16-bit variable from overflow - sendDataHead(ID_TEMPRATURE1); - serialize16(tmpi); + serialize16((uint16_t)frskyGetFlightMode()); } #ifdef USE_GPS static void sendSatalliteSignalQualityAsTemperature2(void) { - uint32_t tmpi = 0; - - // ones and tens columns (# of satellites 0 - 99) - tmpi += constrain(gpsSol.numSat, 0, 99); - - // hundreds column (satellite accuracy HDOP: 0 = worst, 9 = best) - tmpi += (9 - constrain(gpsSol.hdop / 1000, 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; - sendDataHead(ID_TEMPRATURE2); - serialize16(tmpi); + serialize16(frskyGetGPSState()); } static void sendSpeed(void) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 14f6ab3246a..bf47b4245c6 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -56,6 +56,7 @@ #include "telemetry/telemetry.h" #include "telemetry/smartport.h" +#include "telemetry/frsky.h" #include "telemetry/msp_shared.h" #define SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500 @@ -427,73 +428,14 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear break; case FSSP_DATAID_T1 : { - uint32_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_WP_MODE)) - tmpi += 2000; - if (FLIGHT_MODE(HEADFREE_MODE)) - tmpi += 4000; - - // ten thousands column - if (FLIGHT_MODE(FLAPERON)) - tmpi += 10000; - if (FLIGHT_MODE(AUTO_TUNE)) - tmpi += 20000; - if (FLIGHT_MODE(FAILSAFE_MODE)) - tmpi += 40000; - - smartPortSendPackage(id, (uint32_t)tmpi); + smartPortSendPackage(id, frskyGetFlightMode()); *clearToSend = false; break; } case FSSP_DATAID_T2 : if (sensors(SENSOR_GPS)) { #ifdef USE_GPS - uint32_t tmpi = 0; - - // ones and tens columns (# of satellites 0 - 99) - tmpi += constrain(gpsSol.numSat, 0, 99); - - // hundreds column (satellite accuracy HDOP: 0 = worst, 9 = best) - tmpi += (9 - constrain(gpsSol.hdop / 1000, 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; - - smartPortSendPackage(id, tmpi); + smartPortSendPackage(id, frskyGetGPSState()); *clearToSend = false; #endif } else if (feature(FEATURE_GPS)) { From 1e038389f98d52f6fa4777814651546190bfa66f Mon Sep 17 00:00:00 2001 From: Alexey Stankevich Date: Mon, 7 May 2018 23:40:37 +0300 Subject: [PATCH 5/9] Prevent 16-bit overflow --- src/main/telemetry/frsky.c | 11 +++++------ src/main/telemetry/frsky.h | 2 +- src/main/telemetry/frsky_d.c | 2 +- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 2c1000f94a9..24167444ed9 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -16,9 +16,9 @@ #include "io/gps.h" #include "telemetry/frsky.h" -uint32_t frskyGetFlightMode(void) +uint16_t frskyGetFlightMode(void) { - uint32_t tmpi = 0; + uint16_t tmpi = 0; // ones column if (!isArmingDisabled()) @@ -55,12 +55,11 @@ uint32_t frskyGetFlightMode(void) // ten thousands column if (FLIGHT_MODE(FLAPERON)) tmpi += 10000; - if (FLIGHT_MODE(AUTO_TUNE)) - tmpi += 20000; if (FLIGHT_MODE(FAILSAFE_MODE)) tmpi += 40000; - // todo: prevent 16-bit variable from overflow, - // D-series receivers transmit only 16-bit values + else + if (FLIGHT_MODE(AUTO_TUNE)) + tmpi += 20000; return tmpi; } diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h index 3ead24cc559..d112390efe0 100644 --- a/src/main/telemetry/frsky.h +++ b/src/main/telemetry/frsky.h @@ -5,5 +5,5 @@ #pragma once -uint32_t frskyGetFlightMode(void); +uint16_t frskyGetFlightMode(void); uint16_t frskyGetGPSState(void); diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c index 8cfde87182a..cda5468a1c6 100644 --- a/src/main/telemetry/frsky_d.c +++ b/src/main/telemetry/frsky_d.c @@ -207,7 +207,7 @@ static void sendThrottleOrBatterySizeAsRpm(void) static void sendFlightModeAsTemperature1(void) { sendDataHead(ID_TEMPRATURE1); - serialize16((uint16_t)frskyGetFlightMode()); + serialize16(frskyGetFlightMode()); } #ifdef USE_GPS From f5b64d96a70b055179566662c00e0b19cafa57e9 Mon Sep 17 00:00:00 2001 From: Alexey Stankevich Date: Thu, 10 May 2018 00:07:32 +0300 Subject: [PATCH 6/9] Fix formatting and comment --- src/main/telemetry/frsky.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 24167444ed9..d564f541506 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -57,9 +57,8 @@ uint16_t frskyGetFlightMode(void) tmpi += 10000; if (FLIGHT_MODE(FAILSAFE_MODE)) tmpi += 40000; - else - if (FLIGHT_MODE(AUTO_TUNE)) - tmpi += 20000; + else if (FLIGHT_MODE(AUTO_TUNE)) // intentionally reverse order and 'else-if' to prevent 16-bit overflow + tmpi += 20000; return tmpi; } From 13eaaa2713c620b418e2ed3a5405d272fc84fcea Mon Sep 17 00:00:00 2001 From: Alexey Stankevich Date: Thu, 21 Jun 2018 23:03:24 +0300 Subject: [PATCH 7/9] Resolve conflict with #3207 PR in smartport.c --- src/main/telemetry/smartport.c | 33 +++++++++++++++++++++------------ 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index bf47b4245c6..20691103747 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -232,6 +232,11 @@ void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port) } } +bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload) +{ + return payload && (payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT); +} + void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum) { uint8_t *data = (uint8_t *)payload; @@ -326,6 +331,15 @@ static void smartPortSendMspResponse(uint8_t *data) { } #endif +static bool smartPortShouldSendGPSData(void) +{ + // We send GPS data if the GPS is configured and we have a fix + // or the craft has never been armed yet. This way if GPS stops working + // while in flight, the user will easily notice because the sensor will stop + // updating. + return feature(FEATURE_GPS) && (STATE(GPS_FIX) || !ARMING_FLAG(WAS_EVER_ARMED)); +} + void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout) { if (payload) { @@ -333,7 +347,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear // unless we start receiving other sensors' packets #if defined(USE_MSP_OVER_TELEMETRY) - if (payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT) { + if (smartPortPayloadContainsMSP(payload)) { // Pass only the payload: skip frameId uint8_t *frameStart = (uint8_t *)&payload->valueId; smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE); @@ -432,20 +446,15 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear *clearToSend = false; break; } - case FSSP_DATAID_T2 : - if (sensors(SENSOR_GPS)) { #ifdef USE_GPS + case FSSP_DATAID_T2 : + if (smartPortShouldSendGPSData()) { smartPortSendPackage(id, frskyGetGPSState()); *clearToSend = false; -#endif - } else if (feature(FEATURE_GPS)) { - smartPortSendPackage(id, 0); - *clearToSend = false; } break; -#ifdef USE_GPS case FSSP_DATAID_SPEED : - if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { + if (smartPortShouldSendGPSData()) { //convert to knots: 1cm/s = 0.0194384449 knots //Speed should be sent in knots/1000 (GPS speed is in cm/s) uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100; @@ -454,7 +463,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear } break; case FSSP_DATAID_LATLONG : - if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { + if (smartPortShouldSendGPSData()) { uint32_t tmpui = 0; // the same ID is sent twice, one for longitude, one for latitude // the MSB of the sent uint32_t helps FrSky keep track @@ -474,13 +483,13 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear } break; case FSSP_DATAID_HOME_DIST : - if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { + if (smartPortShouldSendGPSData()) { smartPortSendPackage(id, GPS_distanceToHome); *clearToSend = false; } break; case FSSP_DATAID_GPS_ALT : - if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { + if (smartPortShouldSendGPSData()) { smartPortSendPackage(id, gpsSol.llh.alt); // cm *clearToSend = false; } From 04479ae17454fec19c3f97801bd8272a06cc35fa Mon Sep 17 00:00:00 2001 From: Alexey Stankevich Date: Thu, 21 Jun 2018 23:35:00 +0300 Subject: [PATCH 8/9] Add Battery profile feature to frsky_d.c --- src/main/telemetry/frsky_d.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c index cda5468a1c6..ed6860b8970 100644 --- a/src/main/telemetry/frsky_d.c +++ b/src/main/telemetry/frsky_d.c @@ -199,7 +199,7 @@ static void sendThrottleOrBatterySizeAsRpm(void) throttleForRPM = 0; serialize16(throttleForRPM); } else { - serialize16((batteryConfig()->capacity.value / BLADE_NUMBER_DIVIDER)); + serialize16((currentBatteryProfile->capacity.value / BLADE_NUMBER_DIVIDER)); } } From e7faf259177149ee688bf9801962d27226988c4c Mon Sep 17 00:00:00 2001 From: Alexey Stankevich Date: Thu, 21 Jun 2018 23:52:16 +0300 Subject: [PATCH 9/9] Add frsky_d.c to source.mk --- make/source.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/make/source.mk b/make/source.mk index 43197df0419..5d7049a69c8 100644 --- a/make/source.mk +++ b/make/source.mk @@ -173,6 +173,7 @@ COMMON_SRC = \ sensors/opflow.c \ telemetry/crsf.c \ telemetry/frsky.c \ + telemetry/frsky_d.c \ telemetry/hott.c \ telemetry/ibus_shared.c \ telemetry/ibus.c \