piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

GPS distance

Browse Source
This commit is contained in:
bsongis
2012-02-10 09:24:32 +00:00
parent 80ee43db71
commit 0e3675a1b6
7 changed files with 35 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/frsky.cpp
+27 -15
View File
@@ -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
src/frsky.h
+1 -1
View File
@@ -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
src/main_views.cpp
+3 -5
View File
@@ -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) {
src/o9xstrings.cpp
+1 -1
View File
@@ -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;
src/o9xstrings.h
+1 -1
View File
@@ -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[];
src/translations/en.h
+1 -1
View File
@@ -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:"
src/translations/fr.h
+1 -1
View File
@@ -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:"