From 8aee92218d8998fa8fb1153a53b197db28977d08 Mon Sep 17 00:00:00 2001 From: bsongis Date: Thu, 9 Feb 2012 22:56:09 +0000 Subject: [PATCH] GPS distance algorithm. To be used later. --- src/frsky.cpp | 71 ++++++++++++++++++++++++++++++++++++++++++---- src/frsky.h | 16 ++++++++--- src/main_views.cpp | 6 ++++ 3 files changed, 84 insertions(+), 9 deletions(-) diff --git a/src/frsky.cpp b/src/frsky.cpp index dde202db8..f0956d5aa 100644 --- a/src/frsky.cpp +++ b/src/frsky.cpp @@ -92,15 +92,13 @@ uint8_t frskyGetUserData(char *buffer, uint8_t bufSize) #endif #ifdef FRSKY_HUB -int8_t parseTelemHubIndex(uint8_t index) +int8_t parseTelemHubIndex(int8_t index) { if (index > 0x3b) - index = 0; // invalid index + index = -1; // invalid index if (index > 0x39) index -= 17; - if (index > 0x21) - index -= 5; - return 2*(index-1); + return 2*index; } typedef enum { @@ -516,15 +514,78 @@ void resetTelemetry() memset(frskyRSSI, 0, sizeof(frskyRSSI)); #if defined(FRSKY_HUB) || defined(WS_HOW_HIGH) frskyHubData.baroAltitudeOffset = -frskyHubData.baroAltitude; +#endif + +#ifdef SIMU + frskyHubData.gpsLatitude_bp = 4401; + frskyHubData.gpsLatitude_ap = 7710; + frskyHubData.gpsLongitude_bp = 1006; + frskyHubData.gpsLongitude_ap = 8872; +#endif + +#if defined(FRSKY_HUB) frskyHubData.maxGpsSpeed = 0; frskyHubData.cellsCount = 0; frskyHubData.minCellVolts = 0; + if (frskyHubData.gpsLatitude_ap) { + 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; + uint32_t angle2 = (frskyHubData.gpsLatitude_bp*frskyHubData.gpsLatitude_bp) / 10000; + uint32_t angle4 = angle2 * angle2; + frskyHubData.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500); + // printf("frskyHubData.distFromEarthAxis=%d\n", frskyHubData.distFromEarthAxis); fflush(stdout); + } #endif #ifdef SIMU frskyTelemetry[0].set(120); frskyRSSI[0].set(75); frskyHubData.fuelLevel = 75; + frskyHubData.gpsLatitude_bp = 4401; + frskyHubData.gpsLatitude_ap = 7455; + frskyHubData.gpsLongitude_bp = 1006; + frskyHubData.gpsLongitude_ap = 9533; #endif } +#ifdef FRSKY_HUB + +uint16_t sqrt32(uint32_t n) +{ + unsigned int c = 0x8000; + unsigned int g = 0x8000; + + for(;;) { + if(g*g > n) + g ^= c; + c >>= 1; + if(c == 0) + return g; + g |= c; + } +} + +uint32_t getGpsDistanceX2() +{ + uint32_t lat = (((frskyHubData.gpsLatitude_bp % 100) * 10000 + frskyHubData.gpsLatitude_ap) * 5) / 3; + uint32_t lng = (((frskyHubData.gpsLongitude_bp % 100) * 10000 + frskyHubData.gpsLongitude_ap) * 5) / 3; + + // 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; + + angle = (lng > frskyHubData.pilotLongitude) ? lng - frskyHubData.pilotLongitude : frskyHubData.pilotLongitude - lng; + uint32_t dist2 = frskyHubData.distFromEarthAxis * angle / 1000000; + + // printf("dist1=%d, dist2=%d\n", dist1, dist2); + + return dist1*dist1+dist2*dist2; +} + +uint32_t getGpsDistance() +{ + return sqrt32(getGpsDistanceX2()); +} + +#endif diff --git a/src/frsky.h b/src/frsky.h index fe981178c..ee2afd645 100644 --- a/src/frsky.h +++ b/src/frsky.h @@ -53,18 +53,19 @@ class FrskyData: public FrskyRSSI { void set(uint8_t value); }; +#define EARTH_RADIUSKM ((uint32_t)6371) +#define EARTH_RADIUS ((uint32_t)111194) #if defined(FRSKY_HUB) struct FrskyHubData { - // 1 spare + int16_t baroAltitudeOffset; // spare reused int16_t gpsAltitude_bp; // before punct int16_t temperature1; // -20 .. 250 deg. celcius uint16_t rpm; // 0..60,000 revs. per minute uint16_t fuelLevel; // 0, 25, 50, 75, 100 percent int16_t temperature2; // -20 .. 250 deg. celcius uint16_t volts; // 1/500V increments (0..4.2V) - int16_t baroAltitudeOffset; // spare reused - uint16_t maxGpsSpeed; // spare reused + uint32_t distFromEarthAxis; // 2 spares reused int16_t gpsAltitude_ap; // after punct uint8_t cellVolts[12]; // 6 spares reused uint16_t baroAltitude; // 0..9,999 meters @@ -82,7 +83,9 @@ struct FrskyHubData { uint16_t gpsLongitude_ap; uint16_t gpsLatitude_ap; uint16_t gpsCourse_ap; - // 5 spares + uint32_t pilotLatitude; // 2 spares reused + uint32_t pilotLongitude; // 2 spares reused + uint16_t maxGpsSpeed; // spare reused uint16_t gpsLongitudeEW; // East/West uint16_t gpsLatitudeNS; // North/South int16_t accelX; // 1/256th gram (-8g ~ +8g) @@ -140,5 +143,10 @@ bool FRSKY_alarmRaised(uint8_t idx); void resetTelemetry(); +#ifdef FRSKY_HUB +uint32_t getGpsDistanceX2(); +uint32_t getGpsDistance(); +#endif + #endif diff --git a/src/main_views.cpp b/src/main_views.cpp index 24dddfa59..913771222 100644 --- a/src/main_views.cpp +++ b/src/main_views.cpp @@ -419,7 +419,9 @@ void menuMainView(uint8_t event) } } +#ifdef FRSKY_HUB // Cells voltage + // TODO only display used cells! lcd_putsLeft( 5*FH-3, PSTR("V1=\004V2=\004V3=")) ; lcd_putsLeft( 6*FH-3, PSTR("V4=\004V5=\004V6=")); { @@ -436,6 +438,7 @@ void menuMainView(uint8_t event) x += 7*FW; } } +#endif displayRssiLine(); } @@ -506,6 +509,9 @@ void menuMainView(uint8_t event) #define ALTITUDE_LINE (4*FH-1) lcd_putsLeft(ALTITUDE_LINE, STR_ALT); 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) { // Temperature 1