AP_NMEA_Output: fix GPGGA hdop, fix, sats

This commit is contained in:
Tom Pittenger 2023-03-13 12:58:16 -07:00 committed by Tom Pittenger
parent 41ba830008
commit 4a2cbf17f9
1 changed files with 66 additions and 22 deletions

View File

@ -25,13 +25,16 @@
#include <AP_Math/definitions.h> #include <AP_Math/definitions.h>
#include <AP_RTC/AP_RTC.h> #include <AP_RTC/AP_RTC.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>
#include <AP_SerialManager/AP_SerialManager.h> #include <AP_SerialManager/AP_SerialManager.h>
#include <AP_Common/NMEA.h> #include <AP_Common/NMEA.h>
#include <stdio.h> #include <stdio.h>
#include <time.h> #include <time.h>
#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_AHRS)
#include <AP_AHRS/AP_AHRS.h>
#endif
#ifndef AP_NMEA_OUTPUT_MESSAGE_ENABLED_DEFAULT #ifndef AP_NMEA_OUTPUT_MESSAGE_ENABLED_DEFAULT
#define AP_NMEA_OUTPUT_MESSAGE_ENABLED_DEFAULT 3 // GPGGA and GPRMC #define AP_NMEA_OUTPUT_MESSAGE_ENABLED_DEFAULT 3 // GPGGA and GPRMC
#endif #endif
@ -106,20 +109,17 @@ void AP_NMEA_Output::update()
char tstring[11]; char tstring[11];
snprintf(tstring, sizeof(tstring), "%02u%02u%06.2f", tm->tm_hour, tm->tm_min, tm->tm_sec + (time_usec % 1000000) * 1.0e-6); snprintf(tstring, sizeof(tstring), "%02u%02u%06.2f", tm->tm_hour, tm->tm_min, tm->tm_sec + (time_usec % 1000000) * 1.0e-6);
// get location (note: get_location from AHRS always returns true after having GPS position once)
Location loc; Location loc;
uint8_t num_sats; const auto &gps = AP::gps();
uint16_t hdop; const AP_GPS::GPS_Status gps_status = gps.status();
uint8_t gps_status;
#ifndef HAL_BUILD_AP_PERIPH #if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_AHRS)
auto &ahrs = AP::ahrs(); auto &ahrs = AP::ahrs();
// NOTE: ahrs.get_location() always returns true after having GPS position once because it will be dead-reckoning
const bool pos_valid = ahrs.get_location(loc); const bool pos_valid = ahrs.get_location(loc);
#else #else
const bool pos_valid = AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D; const bool pos_valid = (gps_status >= AP_GPS::GPS_OK_FIX_3D);
loc = AP::gps().location(); loc = gps.location();
num_sats = AP::gps().num_sats();
hdop = AP::gps().get_hdop();
gps_status = AP::gps().status();
#endif #endif
// format latitude // format latitude
@ -149,14 +149,61 @@ void AP_NMEA_Output::update()
uint16_t gga_length = 0; uint16_t gga_length = 0;
if ((_message_enable_bitmask.get() & static_cast<int16_t>(Enabled_Messages::GPGGA)) != 0) { if ((_message_enable_bitmask.get() & static_cast<int16_t>(Enabled_Messages::GPGGA)) != 0) {
// format GGA message // format GGA message
// Convert AP_GPS::GPS_Status:
// 0 = NO_GPS
// 1 = NO_FIX
// 2 = GPS_OK_FIX_2D
// 3 = GPS_OK_FIX_3D
// 4 = GPS_OK_FIX_3D_DGPS
// 5 = GPS_OK_FIX_3D_RTK_FLOAT
// 6 = GPS_OK_FIX_3D_RTK_FIXED
// To NMEA "Fix Quality" per Trimble definition:
// 0: Fix not valid
// 1: GPS fix
// 2: Differential GPS fix (DGNSS), SBAS, OmniSTAR VBS, Beacon, RTX in GVBS mode
// 3: Not applicable
// 4: RTK Fixed, xFill
// 5: RTK Float, OmniSTAR XP/HP, Location RTK, RTX
// 6: INS Dead reckoning
// sources:
// https://orolia.com/manuals/VSP/Content/NC_and_SS/Com/Topics/APPENDIX/NMEA_GGAmess.htm
// http://aprs.gids.nl/nmea/#gga
// https://docs.novatel.com/OEM7/Content/Logs/GPGGA.htm
// https://receiverhelp.trimble.com/alloy-gnss/en-us/NMEA-0183messages_GGA.html <-using this one
uint8_t fix_quality;
switch (gps_status) {
default:
case AP_GPS::NO_GPS:
case AP_GPS::NO_FIX:
case AP_GPS::GPS_OK_FIX_2D:
// NOTE: ahrs.get_location() always returns pos_valid=true after having GPS position once because it will be dead-reckoning
fix_quality = pos_valid ? 6 : 0;
break;
case AP_GPS::GPS_OK_FIX_3D:
fix_quality = 1;
break;
case AP_GPS::GPS_OK_FIX_3D_DGPS:
fix_quality = 2;
break;
case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT:
fix_quality = 5;
break;
case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED:
fix_quality = 4;
break;
}
gga_length = nmea_printf_buffer(gga, sizeof(gga), gga_length = nmea_printf_buffer(gga, sizeof(gga),
"$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.1f,M,0.0,M,,", "$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,",
tstring, tstring,
lat_string, lat_string,
lng_string, lng_string,
status, fix_quality,
num_sats, gps.num_sats(),
hdop, gps.get_hdop(),
loc.alt * 0.01f); loc.alt * 0.01f);
space_required += gga_length; space_required += gga_length;
@ -170,17 +217,15 @@ void AP_NMEA_Output::update()
snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100); snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100);
// get speed // get speed
#ifndef HAL_BUILD_AP_PERIPH #if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_AHRS)
const Vector2f speed = ahrs.groundspeed_vector(); const Vector2f speed = ahrs.groundspeed_vector();
const float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS; const float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
const float heading = wrap_360(degrees(atan2f(speed.x, speed.y))); const float heading = wrap_360(degrees(atan2f(speed.x, speed.y)));
#else #else
const float speed_knots = AP::gps().ground_speed() * M_PER_SEC_TO_KNOTS; const float speed_knots = gps.ground_speed() * M_PER_SEC_TO_KNOTS;
const float heading = AP::gps().ground_course(); const float heading = gps.ground_course();
#endif #endif
// format RMC message // format RMC message
rmc_length = nmea_printf_buffer(rmc, sizeof(rmc), rmc_length = nmea_printf_buffer(rmc, sizeof(rmc),
"$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,", "$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,",
@ -197,7 +242,7 @@ void AP_NMEA_Output::update()
uint16_t pashr_length = 0; uint16_t pashr_length = 0;
char pashr[100]; char pashr[100];
#ifndef HAL_BUILD_AP_PERIPH #if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_AHRS)
if ((_message_enable_bitmask.get() & static_cast<int16_t>(Enabled_Messages::PASHR)) != 0) { if ((_message_enable_bitmask.get() & static_cast<int16_t>(Enabled_Messages::PASHR)) != 0) {
// get roll, pitch, yaw // get roll, pitch, yaw
const float roll_deg = wrap_180(degrees(ahrs.get_roll())); const float roll_deg = wrap_180(degrees(ahrs.get_roll()));
@ -212,7 +257,6 @@ void AP_NMEA_Output::update()
// 0 = no position // 0 = no position
// 1 = All non-RTK fixed integer positions // 1 = All non-RTK fixed integer positions
// 2 = RTK fixed integer positions // 2 = RTK fixed integer positions
const AP_GPS::GPS_Status gps_status = AP::gps().status();
const uint8_t gps_status_flag = (gps_status >= AP_GPS::GPS_OK_FIX_3D_RTK_FIXED) ? 2 : const uint8_t gps_status_flag = (gps_status >= AP_GPS::GPS_OK_FIX_3D_RTK_FIXED) ? 2 :
(gps_status >= AP_GPS::GPS_OK_FIX_2D ? 1 : 0); (gps_status >= AP_GPS::GPS_OK_FIX_2D ? 1 : 0);