[gps] Add horizontal and vertical position accuracy

paparazzi · Mar 17, 2019 · c13aa87 · c13aa87
1 parent 5cccc06
commit c13aa87
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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);