From c13aa8721f8470a8f4eb95a91ff868dbed143c66 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Tue, 5 Mar 2019 02:17:24 -0800 Subject: [PATCH] [gps] Add horizontal and vertical position accuracy --- sw/airborne/subsystems/gps.c | 2 ++ sw/airborne/subsystems/gps.h | 2 ++ sw/airborne/subsystems/gps/gps_ubx.c | 2 ++ 3 files changed, 6 insertions(+) diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index 65038b11bc3..fa2400558ce 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -321,6 +321,8 @@ void gps_init(void) gps.week = 0; gps.tow = 0; gps.cacc = 0; + gps.hacc = 0; + gps.vacc = 0; gps.last_3dfix_ticks = 0; gps.last_3dfix_time = 0; diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index ce469a5e204..dec65d50c69 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -92,6 +92,8 @@ struct GpsState { uint16_t speed_3d; ///< norm of 3d speed in cm/s int32_t course; ///< GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north) uint32_t pacc; ///< position accuracy in cm + uint32_t hacc; ///< horizontal accuracy in cm + uint32_t vacc; ///< vertical accuracy in cm uint32_t sacc; ///< speed accuracy in cm/s uint32_t cacc; ///< course accuracy in rad*1e7 uint16_t pdop; ///< position dilution of precision scaled by 100 diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 3e30779dcc9..45e51ad3451 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -155,6 +155,8 @@ void gps_ubx_read_message(void) gps_ubx.state.lla_pos.lat = UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf); gps_ubx.state.lla_pos.lon = UBX_NAV_POSLLH_LON(gps_ubx.msg_buf); gps_ubx.state.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf); + gps_ubx.state.hacc = UBX_NAV_POSLLH_Hacc(gps_ubx.msg_buf); + gps_ubx.state.vacc = UBX_NAV_POSLLH_Vacc(gps_ubx.msg_buf); SetBit(gps_ubx.state.valid_fields, GPS_VALID_POS_LLA_BIT); gps_ubx.state.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf); SetBit(gps_ubx.state.valid_fields, GPS_VALID_HMSL_BIT);