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

GPS maths prepared on GPS fix. Still needs to be tested!

Browse Source
This commit is contained in:
bsongis
2012-02-09 23:04:08 +00:00
parent 8aee92218d
commit 80ee43db71
2 changed files with 15 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/frsky.cpp
+14 -6
View File
@@ -158,6 +158,9 @@ 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;
}
@@ -528,12 +531,7 @@ void resetTelemetry()
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);
getGpsPilotCoords();
}
#endif
@@ -565,6 +563,16 @@ uint16_t sqrt32(uint32_t n)
}
}
void getGpsPilotCoords()
{
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);
}
uint32_t getGpsDistanceX2()
{
uint32_t lat = (((frskyHubData.gpsLatitude_bp % 100) * 10000 + frskyHubData.gpsLatitude_ap) * 5) / 3;