From 0e3675a1b6d4ee904749e41d8a25846fb4264519 Mon Sep 17 00:00:00 2001 From: bsongis Date: Fri, 10 Feb 2012 09:24:32 +0000 Subject: [PATCH] GPS distance --- src/frsky.cpp | 42 +++++++++++++++++++++++++++--------------- src/frsky.h | 2 +- src/main_views.cpp | 8 +++----- src/o9xstrings.cpp | 2 +- src/o9xstrings.h | 2 +- src/translations/en.h | 2 +- src/translations/fr.h | 2 +- 7 files changed, 35 insertions(+), 25 deletions(-) diff --git a/src/frsky.cpp b/src/frsky.cpp index 25e2eb9af..6d2e9c39b 100644 --- a/src/frsky.cpp +++ b/src/frsky.cpp @@ -142,11 +142,23 @@ void parseTelemHubByte(uint8_t byte) return; } ((uint8_t*)&frskyHubData)[structPos+1] = byte; + + if ((uint8_t)structPos == offsetof(FrskyHubData, baroAltitude) && !frskyHubData.baroAltitudeOffset) { + // First received altitude => Altitude offset + frskyHubData.baroAltitudeOffset = -frskyHubData.baroAltitude; + } + + if ((uint8_t)structPos == offsetof(FrskyHubData, gpsLatitudeNS/*TODO check that it is received at last!*/) && !frskyHubData.pilotLatitude) { + // First received GPS position => Pilot GPS position + getGpsPilotPosition(); + } + if ((uint8_t)structPos == offsetof(FrskyHubData, gpsSpeed_ap)) { // Speed => Max speed if (frskyHubData.maxGpsSpeed < frskyHubData.gpsSpeed_ap) frskyHubData.maxGpsSpeed = frskyHubData.gpsSpeed_ap; } + if ((uint8_t)structPos == offsetof(FrskyHubData, volts)) { // Voltage => Cell number + Cell voltage uint8_t battnumber = (frskyHubData.volts & 0x00F0) >> 4; @@ -158,9 +170,6 @@ void parseTelemHubByte(uint8_t byte) if (!frskyHubData.minCellVolts || cellVolts < frskyHubData.minCellVolts) frskyHubData.minCellVolts = cellVolts; } - if ((uint8_t)structPos == offsetof(FrskyHubData, gpsLatitudeNS/*TODO check that it is received at last!*/) && !frskyHubData.pilotLatitude) { - getGpsPilotCoords(); - } state = TS_IDLE; } @@ -515,9 +524,6 @@ void resetTelemetry() { memset(frskyTelemetry, 0, sizeof(frskyTelemetry)); memset(frskyRSSI, 0, sizeof(frskyRSSI)); -#if defined(FRSKY_HUB) || defined(WS_HOW_HIGH) - frskyHubData.baroAltitudeOffset = -frskyHubData.baroAltitude; -#endif #ifdef SIMU frskyHubData.gpsLatitude_bp = 4401; @@ -526,12 +532,17 @@ void resetTelemetry() frskyHubData.gpsLongitude_ap = 8872; #endif +#if defined(FRSKY_HUB) || defined(WS_HOW_HIGH) + frskyHubData.baroAltitudeOffset = -frskyHubData.baroAltitude; +#endif + + #if defined(FRSKY_HUB) frskyHubData.maxGpsSpeed = 0; frskyHubData.cellsCount = 0; frskyHubData.minCellVolts = 0; if (frskyHubData.gpsLatitude_ap) { - getGpsPilotCoords(); + getGpsPilotPosition(); } #endif @@ -547,7 +558,6 @@ void resetTelemetry() } #ifdef FRSKY_HUB - uint16_t sqrt32(uint32_t n) { unsigned int c = 0x8000; @@ -563,7 +573,7 @@ uint16_t sqrt32(uint32_t n) } } -void getGpsPilotCoords() +void getGpsPilotPosition() { frskyHubData.pilotLatitude = /*((uint32_t)frskyHubData.gpsLatitude_bp << 16) + */(((frskyHubData.gpsLatitude_bp % 100) * 10000 + frskyHubData.gpsLatitude_ap) * 5) / 3; frskyHubData.pilotLongitude = /*((uint32_t)frskyHubData.gpsLongitude_bp << 16) + */(((frskyHubData.gpsLongitude_bp % 100) * 10000 + frskyHubData.gpsLongitude_ap) * 5) / 3; @@ -581,19 +591,21 @@ uint32_t getGpsDistanceX2() // printf("lat=%d (%d), long=%d (%d)\n", lat, abs(lat - frskyHubData.pilotLatitude), lng, abs(lng - frskyHubData.pilotLongitude)); uint32_t angle = (lat > frskyHubData.pilotLatitude) ? lat - frskyHubData.pilotLatitude : frskyHubData.pilotLatitude - lat; - uint32_t dist1 = EARTH_RADIUS * angle / 1000000; + uint32_t dist = EARTH_RADIUS * angle / 1000000; + uint32_t result = dist*dist; angle = (lng > frskyHubData.pilotLongitude) ? lng - frskyHubData.pilotLongitude : frskyHubData.pilotLongitude - lng; - uint32_t dist2 = frskyHubData.distFromEarthAxis * angle / 1000000; + dist = frskyHubData.distFromEarthAxis * angle / 1000000; + result += dist*dist; - // printf("dist1=%d, dist2=%d\n", dist1, dist2); + dist = frskyHubData.baroAltitude + frskyHubData.baroAltitudeOffset; + result += dist*dist; - return dist1*dist1+dist2*dist2; + return result; } uint32_t getGpsDistance() { - return sqrt32(getGpsDistanceX2()); + return (frskyHubData.pilotLatitude ? sqrt32(getGpsDistanceX2()) : 0); } - #endif diff --git a/src/frsky.h b/src/frsky.h index 2d1fcbc5a..40b9da23d 100644 --- a/src/frsky.h +++ b/src/frsky.h @@ -144,7 +144,7 @@ bool FRSKY_alarmRaised(uint8_t idx); void resetTelemetry(); #ifdef FRSKY_HUB -void getGpsPilotCoords(); +void getGpsPilotPosition(); uint32_t getGpsDistanceX2(); uint32_t getGpsDistance(); #endif diff --git a/src/main_views.cpp b/src/main_views.cpp index 913771222..4ce4cf45c 100644 --- a/src/main_views.cpp +++ b/src/main_views.cpp @@ -104,7 +104,7 @@ void displayRssiLine() #if defined(FRSKY_HUB) || defined(WS_HOW_HIGH) void displayAltitudeLine(uint8_t x, uint8_t y, uint8_t flags) { - lcd_puts(x, y, STR_ALT); + lcd_putsn_P(x, y, STR_ALTnDST, 4); int16_t value = frskyHubData.baroAltitude + frskyHubData.baroAltitudeOffset; putsTelemetryValue(lcd_lastPos, y, value, UNIT_METERS, flags|LEFT); } @@ -505,12 +505,10 @@ void menuMainView(uint8_t event) putsTelemetryValue(4*FW, SPEED_LINE, frskyHubData.gpsSpeed_bp, UNIT_KTS, LEFT|DBLSIZE); // before '.' putsTelemetryValue(16*FW, SPEED_LINE, frskyHubData.maxGpsSpeed, UNIT_KTS, LEFT); // before '.' - // Altitude + // GPS altitude and distance #define ALTITUDE_LINE (4*FH-1) - lcd_putsLeft(ALTITUDE_LINE, STR_ALT); + lcd_putsLeft(ALTITUDE_LINE, STR_ALTnDST); putsTelemetryValue(4*FW, ALTITUDE_LINE, frskyHubData.gpsAltitude_bp, UNIT_METERS, LEFT); // before '.' - - lcd_puts(12*FW, ALTITUDE_LINE, PSTR("Dst:")); putsTelemetryValue(16*FW, ALTITUDE_LINE, getGpsDistance(), UNIT_METERS, LEFT); // before '.' } else if (g_model.frsky.usrProto == PROTO_FRSKY_HUB && view == e_telemetry+3*ALTERNATE_VIEW) { diff --git a/src/o9xstrings.cpp b/src/o9xstrings.cpp index 80fadd28d..cd5a43ba0 100644 --- a/src/o9xstrings.cpp +++ b/src/o9xstrings.cpp @@ -141,7 +141,7 @@ const pm_char STR_BG[] PROGMEM = TR_BG; const pm_char STR_MENUTOSTART[] PROGMEM = TR_MENUTOSTART; const pm_char STR_SETMIDPOINT[] PROGMEM = TR_SETMIDPOINT; const pm_char STR_MOVESTICKSPOTS[] PROGMEM = TR_MOVESTICKSPOTS; -const pm_char STR_ALT[] PROGMEM = TR_ALT; +const pm_char STR_ALTnDST[] PROGMEM = TR_ALTnDST; const pm_char STR_RXBATT[] PROGMEM = TR_RXBATT; const pm_char STR_RX[] PROGMEM = TR_RX; const pm_char STR_TX[] PROGMEM = TR_TX; diff --git a/src/o9xstrings.h b/src/o9xstrings.h index 11c544380..3a6735a06 100644 --- a/src/o9xstrings.h +++ b/src/o9xstrings.h @@ -222,7 +222,7 @@ extern const pm_char STR_BG[]; extern const pm_char STR_MENUTOSTART[]; extern const pm_char STR_SETMIDPOINT[]; extern const pm_char STR_MOVESTICKSPOTS[]; -extern const pm_char STR_ALT[]; +extern const pm_char STR_ALTnDST[]; extern const pm_char STR_RXBATT[]; extern const pm_char STR_RX[]; extern const pm_char STR_TX[]; diff --git a/src/translations/en.h b/src/translations/en.h index e3cecd84d..b3f1e39a1 100644 --- a/src/translations/en.h +++ b/src/translations/en.h @@ -214,7 +214,7 @@ #define TR_MENUTOSTART "[MENU] TO START" #define TR_SETMIDPOINT "SET MIDPOINT" #define TR_MOVESTICKSPOTS "MOVE STICKS/POTS" -#define TR_ALT "Alt:" +#define TR_ALTnDST "Alt:\010Dst:" #define TR_RXBATT "Rx Batt:" #define TR_RX "Rx:" #define TR_TX "Tx:" diff --git a/src/translations/fr.h b/src/translations/fr.h index 8fafcf22f..70640a15b 100644 --- a/src/translations/fr.h +++ b/src/translations/fr.h @@ -214,7 +214,7 @@ #define TR_MENUTOSTART "[MENU]POUR DEBUT" #define TR_SETMIDPOINT "SET NEUTRE" #define TR_MOVESTICKSPOTS "BOUGER MANCHES/POTS " -#define TR_ALT "Alt:" +#define TR_ALTnDST "Alt:\010Dst:" #define TR_RXBATT "Rx Batt:" #define TR_RX "Rx:" #define TR_TX "Tx:"