rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #6096 from AirBreak69/GPS_handle_negative_altitude_and_safer_maths_macros 

GPS: handle negative and high altitudes; safer macros in maths.h
This commit is contained in:
Michael Keller 2018-06-20 01:01:02 +12:00 committed by GitHub
parent 1757ec5d16 91ac74ca9c
commit c6a5ab9b96
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 23 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/main/common/maths.h
View File

@ -35,13 +35,13 @@
#define M_PIf 3.14159265358979323846f
#define RAD (M_PIf / 180.0f)
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
#define DECIDEGREES_TO_DEGREES(angle) (angle / 10)
#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f)
#define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10)
#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10)
#define DECIDEGREES_TO_RADIANS(angle) ((angle) / 10.0f * 0.0174532925f)
#define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f)
#define CM_S_TO_KM_H(centimetersPerSecond) (centimetersPerSecond * 36 / 1000)
#define CM_S_TO_MPH(centimetersPerSecond) (((centimetersPerSecond * 10000) / 5080) / 88)
#define CM_S_TO_KM_H(centimetersPerSecond) ((centimetersPerSecond) * 36 / 1000)
#define CM_S_TO_MPH(centimetersPerSecond) ((centimetersPerSecond) * 10000 / 5080 / 88)
#define MIN(a,b) \
__extension__ ({ __typeof__ (a) _a = (a); \

4
src/main/interface/msp.c
View File

@ -1033,7 +1033,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, gpsSol.numSat);
sbufWriteU32(dst, gpsSol.llh.lat);
sbufWriteU32(dst, gpsSol.llh.lon);
sbufWriteU16(dst, gpsSol.llh.alt);
sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.alt / 100, 0, UINT16_MAX)); // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. To maintain backwards compatibility compensate to 1m per lsb in MSP again.
sbufWriteU16(dst, gpsSol.groundSpeed);
sbufWriteU16(dst, gpsSol.groundCourse);
break;
@ -1899,7 +1899,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
gpsSol.numSat = sbufReadU8(src);
gpsSol.llh.lat = sbufReadU32(src);
gpsSol.llh.lon = sbufReadU32(src);
gpsSol.llh.alt = sbufReadU16(src);
gpsSol.llh.alt = sbufReadU16(src) * 100; // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. Received MSP altitudes in 1m per lsb have to upscaled.
gpsSol.groundSpeed = sbufReadU16(src);
GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
break;

22
src/main/io/gps.c
View File

@ -644,28 +644,36 @@ static uint32_t grab_fields(char *src, uint8_t mult)
{ // convert string to uint32
uint32_t i;
uint32_t tmp = 0;
int isneg = 0;
for (i = 0; src[i] != 0; i++) {
if ((i == 0) && (src[0] == '-')) { // detect negative sign
isneg = 1;
continue; // jump to next character if the first one was a negative sign
}
if (src[i] == '.') {
i++;
if (mult == 0)
if (mult == 0) {
break;
else
} else {
src[i + mult] = 0;
}
}
tmp *= 10;
if (src[i] >= '0' && src[i] <= '9')
if (src[i] >= '0' && src[i] <= '9') {
tmp += src[i] - '0';
if (i >= 15)
}
if (i >= 15) {
return 0; // out of bounds
}
}
return tmp;
return isneg ? -tmp : tmp; // handle negative altitudes
}
typedef struct gpsDataNmea_s {
int32_t latitude;
int32_t longitude;
uint8_t numSat;
uint16_t altitude;
int32_t altitude;
uint16_t speed;
uint16_t hdop;
uint16_t ground_course;
@ -736,7 +744,7 @@ static bool gpsNewFrameNMEA(char c)
gps_Msg.hdop = grab_fields(string, 1) * 100; // hdop
break;
case 9:
gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis
gps_Msg.altitude = grab_fields(string, 1) * 10; // altitude in centimeters. Note: NMEA delivers altitude with 1 or 3 decimals. It's safer to cut at 0.1m and multiply by 10
break;
}
break;

3
src/main/io/gps.h
View File

@ -86,12 +86,11 @@ typedef struct gpsCoordinateDDDMMmmmm_s {
typedef struct gpsLocation_s {
int32_t lat; // latitude * 1e+7
int32_t lon; // longitude * 1e+7
int32_t alt; // altitude in 0.1m
int32_t alt; // altitude in 0.01m
} gpsLocation_t;
typedef struct gpsSolutionData_s {
gpsLocation_t llh;
uint16_t GPS_altitude; // altitude in 0.1m
uint16_t groundSpeed; // speed in 0.1m/s
uint16_t groundCourse; // degrees * 10
uint16_t hdop; // generic HDOP value (*100)