1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-16 12:55:12 +03:00

x12 gps improvement (#6570)

* Add hdop handling

* Add GPS to debug screen

* Only update PilotPos when a decent hdop is achieved
This commit is contained in:
3djc 2019-07-14 17:33:31 +02:00 committed by Andre Bernet
parent 328677abed
commit 9792bf52d4
6 changed files with 24 additions and 3 deletions

View file

@ -134,6 +134,7 @@ typedef struct gpsDataNmea_s
uint16_t altitude; uint16_t altitude;
uint16_t speed; uint16_t speed;
uint16_t groundCourse; uint16_t groundCourse;
uint16_t hdop;
uint32_t date; uint32_t date;
uint32_t time; uint32_t time;
} gpsDataNmea_t; } gpsDataNmea_t;
@ -207,6 +208,9 @@ bool gpsNewFrameNMEA(char c)
case 7: case 7:
gps_Msg.numSat = grab_fields(string, 0); gps_Msg.numSat = grab_fields(string, 0);
break; break;
case 8:
gps_Msg.hdop = grab_fields(string, 1) * 10;
break;
case 9: case 9:
gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis
break; break;
@ -258,6 +262,7 @@ bool gpsNewFrameNMEA(char c)
frameOK = 1; frameOK = 1;
gpsData.fix = gps_Msg.fix; gpsData.fix = gps_Msg.fix;
gpsData.numSat = gps_Msg.numSat; gpsData.numSat = gps_Msg.numSat;
gpsData.hdop = gps_Msg.hdop;
if (gps_Msg.fix) { if (gps_Msg.fix) {
__disable_irq(); // do the atomic update of lat/lon __disable_irq(); // do the atomic update of lat/lon
gpsData.latitude = gps_Msg.latitude; gpsData.latitude = gps_Msg.latitude;

View file

@ -35,6 +35,7 @@ struct gpsdata_t
uint16_t altitude; // altitude in 0.1m uint16_t altitude; // altitude in 0.1m
uint16_t speed; // speed in 0.1m/s uint16_t speed; // speed in 0.1m/s
uint16_t groundCourse; // degrees * 10 uint16_t groundCourse; // degrees * 10
uint16_t hdop;
}; };
extern gpsdata_t gpsData; extern gpsdata_t gpsData;

View file

@ -168,7 +168,17 @@ bool menuStatsDebug(event_t event)
lcdDrawText(MENUS_MARGIN_LEFT, y, "Telem RX Errs"); lcdDrawText(MENUS_MARGIN_LEFT, y, "Telem RX Errs");
lcdDrawNumber(MENU_STATS_COLUMN1, y, telemetryErrors, LEFT); lcdDrawNumber(MENU_STATS_COLUMN1, y, telemetryErrors, LEFT);
// y += FH; y += FH;
#if defined(INTERNAL_GPS)
lcdDrawText(MENUS_MARGIN_LEFT, y, "Internal GPS");
lcdDrawText(MENU_STATS_COLUMN1, y+1, "[Fix]", HEADER_COLOR|SMLSIZE);
lcdDrawText(lcdNextPos+2, y, (gpsData.fix ? "Yes" : "No"), LEFT);
lcdDrawText(lcdNextPos+20, y+1, "[Sats]", HEADER_COLOR|SMLSIZE);
lcdDrawNumber(lcdNextPos+5, y, gpsData.numSat, LEFT);
lcdDrawText(lcdNextPos+20, y+1, "[Hdop]", HEADER_COLOR|SMLSIZE);
lcdDrawNumber(lcdNextPos+5, y, gpsData.hdop, PREC2|LEFT);
#endif
lcdDrawText(LCD_W/2, MENU_FOOTER_TOP, STR_MENUTORESET, MENU_TITLE_COLOR | CENTERED); lcdDrawText(LCD_W/2, MENU_FOOTER_TOP, STR_MENUTORESET, MENU_TITLE_COLOR | CENTERED);
return true; return true;

View file

@ -806,6 +806,7 @@ Return the internal GPS position or nil if no valid hardware found
* 'alt' (number) internal GPS altitude in 0.1m * 'alt' (number) internal GPS altitude in 0.1m
* 'speed' (number) internal GPSspeed in 0.1m/s * 'speed' (number) internal GPSspeed in 0.1m/s
* 'heading' (number) internal GPS ground course estimation in degrees * 10 * 'heading' (number) internal GPS ground course estimation in degrees * 10
* 'hdop' (number) internal GPS horizontal dilution of precision
@status current Introduced in 2.2.2 @status current Introduced in 2.2.2
*/ */
@ -819,6 +820,7 @@ static int luaGetTxGPS(lua_State * L)
lua_pushtableinteger(L, "alt", gpsData.altitude); lua_pushtableinteger(L, "alt", gpsData.altitude);
lua_pushtableinteger(L, "speed", gpsData.speed); lua_pushtableinteger(L, "speed", gpsData.speed);
lua_pushtableinteger(L, "heading", gpsData.groundCourse); lua_pushtableinteger(L, "heading", gpsData.groundCourse);
lua_pushtableinteger(L, "hdop", gpsData.hdop);
if (gpsData.fix) if (gpsData.fix)
lua_pushtableboolean(L, "fix", true); lua_pushtableboolean(L, "fix", true);
else else

View file

@ -547,6 +547,9 @@ uint8_t gpsGetByte(uint8_t * byte);
extern uint8_t gpsTraceEnabled; extern uint8_t gpsTraceEnabled;
#endif #endif
void gpsSendByte(uint8_t byte); void gpsSendByte(uint8_t byte);
#if defined(PCBX12S)
#define PILOTPOS_MIN_HDOP 500
#endif
// Second serial port driver // Second serial port driver
#define AUX_SERIAL #define AUX_SERIAL

View file

@ -141,7 +141,7 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32
} }
else if (unit == UNIT_GPS_LATITUDE) { else if (unit == UNIT_GPS_LATITUDE) {
#if defined(INTERNAL_GPS) #if defined(INTERNAL_GPS)
if (gpsData.fix) { if (gpsData.fix && gpsData.hdop < PILOTPOS_MIN_HDOP) {
pilotLatitude = gpsData.latitude; pilotLatitude = gpsData.latitude;
distFromEarthAxis = getDistFromEarthAxis(pilotLatitude); distFromEarthAxis = getDistFromEarthAxis(pilotLatitude);
} }
@ -156,7 +156,7 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32
} }
else if (unit == UNIT_GPS_LONGITUDE) { else if (unit == UNIT_GPS_LONGITUDE) {
#if defined(INTERNAL_GPS) #if defined(INTERNAL_GPS)
if (gpsData.fix) { if (gpsData.fix && gpsData.hdop < PILOTPOS_MIN_HDOP) {
pilotLongitude = gpsData.longitude; pilotLongitude = gpsData.longitude;
} }
#endif #endif