[gps] fix fixedwing UTM position usage

- was correctly used for init but not normal reading - add convenience function to get utm from GPS struct fix stuff missed in #1476
paparazzi · Dec 27, 2015 · 612a4f0 · 612a4f0
1 parent a1a8941
commit 612a4f0
Show file tree

Hide file tree

Showing 10 changed files with 58 additions and 78 deletions.
diff --git a/sw/airborne/modules/cam_control/point.c b/sw/airborne/modules/cam_control/point.c
@@ -408,7 +408,7 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
       struct UtmCoor_f utm;
       utm.east = nav_utm_east0 + fObjectEast;
       utm.north = nav_utm_north0 + fObjectNorth;
-      utm.zone = gps.utm_pos.zone;
+      utm.zone = nav_utm_zone0;
       struct LlaCoor_f lla;
       lla_of_utm_f(&lla, &utm);
       cam_point_lon = lla.lon * (180 / M_PI);

diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
@@ -275,10 +275,7 @@ void ins_xsens_register(void)
 
 void ins_xsens_update_gps(struct GpsState *gps_s)
 {
-  struct UtmCoor_f utm;
-  utm.east = gps_s->utm_pos.east / 100.;
-  utm.north = gps_s->utm_pos.north / 100.;
-  utm.zone = nav_utm_zone0;
+  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
   utm.alt = gps_s->hmsl / 1000.;
 
   // set position

diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c
@@ -206,10 +206,7 @@ void ins_xsens_register(void)
 
 void ins_xsens_update_gps(struct GpsState *gps_s)
 {
-  struct UtmCoor_f utm;
-  utm.east = gps_s->utm_pos.east / 100.;
-  utm.north = gps_s->utm_pos.north / 100.;
-  utm.zone = nav_utm_zone0;
+  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
   utm.alt = gps_s->hmsl / 1000.;
 
   // set position

diff --git a/sw/airborne/modules/meteo/meteo_france_DAQ.c b/sw/airborne/modules/meteo/meteo_france_DAQ.c
@@ -106,10 +106,12 @@ void mf_daq_send_report(void)
     uint8_t foo = 0;
     int16_t climb = -gps.ned_vel.z;
     int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
+    struct UtmCoor_f utm = stateGetPositionEnu_f();
+    int32_t east = utm.east * 100;
+    int32_t north = utm.north * 100;
     DOWNLINK_SEND_GPS(pprzlog_tp, chibios_sdlog, &gps.fix,
-                      &gps.utm_pos.east, &gps.utm_pos.north,
-                      &course, &gps.hmsl, &gps.gspeed, &climb,
-                      &gps.week, &gps.tow, &gps.utm_pos.zone, &foo);
+                      &east, &north, &course, &gps.hmsl, &gps.gspeed, &climb,
+                      &gps.week, &gps.tow, &utm.zone, &foo);
   }
 }
 

diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c
@@ -199,3 +199,33 @@ uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks)
 void WEAK gps_inject_data(uint8_t packet_id __attribute__((unused)), uint8_t length __attribute__((unused)), uint8_t *data __attribute__((unused))){
 
 }
+
+/**
+ * Convenience function to get utm position from GPS state
+ */
+struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
+{
+  struct UtmCoor_f utm;
+  utm.alt = 0.f;
+
+  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
+    // A real UTM position is available, use the correct zone
+    utm.zone = gps_s->utm_pos.zone;
+    utm.east = gps_s->utm_pos.east / 100.0f;
+    utm.north = gps_s->utm_pos.north / 100.0f;
+  }
+  else {
+    struct LlaCoor_f lla;
+    LLA_FLOAT_OF_BFP(lla, gps_s->lla_pos);
+    // Check if zone should be computed
+    if (zone > 0) {
+      utm.zone = zone;
+    } else {
+      utm.zone = (gps_s->lla_pos.lon / 1e7 + 180) / 6 + 1;
+    }
+    /* Recompute UTM coordinates in this zone */
+    utm_of_lla_f(&utm, &lla);
+  }
+
+  return utm;
+}
diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h
@@ -30,6 +30,7 @@
 
 #include "std.h"
 #include "math/pprz_geodetic_int.h"
+#include "math/pprz_geodetic_float.h"
 
 #include "mcu_periph/sys_time.h"
 
@@ -162,4 +163,13 @@ extern struct GpsTimeSync gps_time_sync;
  */
 extern uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks);
 
+/**
+ * Convenience function to get utm position in float from GPS structure.
+ * Beware that altitude is initialized to zero but not set to the correct value
+ * @param[in] gps pointer to the gps structure
+ * @param[in] zone set the utm zone in which the position should be computed, 0 to try to get it automatically from lla position
+ * @return utm position in float
+ */
+extern struct UtmCoor_f utm_float_from_gps(struct GpsState *gps, uint8_t zone);
+
 #endif /* GPS_H */
diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c
@@ -72,20 +72,7 @@ void ins_init(void)
 void WEAK ins_reset_local_origin(void)
 {
 #if USE_GPS
-  struct UtmCoor_f utm;
-
-  if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
-    utm.zone = gps.utm_pos.zone;
-    utm.east = gps.utm_pos.east / 100.0f;
-    utm.north = gps.utm_pos.north / 100.0f;
-  }
-  else {
-    /* Recompute UTM coordinates in this zone */
-    struct LlaCoor_f lla;
-    LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
-    utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
-    utm_of_lla_f(&utm, &lla);
-  }
+  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
 
   // ground_alt
   utm.alt = gps.hmsl  / 1000.0f;

diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c
@@ -113,20 +113,8 @@ void ins_alt_float_init(void)
 /** Reset the geographic reference to the current GPS fix */
 void ins_reset_local_origin(void)
 {
-  struct UtmCoor_f utm;
-
-  if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
-    utm.zone = gps.utm_pos.zone;
-    utm.east = gps.utm_pos.east / 100.0f;
-    utm.north = gps.utm_pos.north / 100.0f;
-  }
-  else {
-    /* Recompute UTM coordinates in this zone */
-    struct LlaCoor_f lla;
-    LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
-    utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
-    utm_of_lla_f(&utm, &lla);
-  }
+  // get utm pos
+  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
 
   // ground_alt
   utm.alt = gps.hmsl  / 1000.0f;
@@ -198,10 +186,7 @@ void ins_alt_float_update_baro(float pressure __attribute__((unused)))
 void ins_alt_float_update_gps(struct GpsState *gps_s)
 {
 #if USE_GPS
-  struct UtmCoor_f utm;
-  utm.east = gps_s->utm_pos.east / 100.0f;
-  utm.north = gps_s->utm_pos.north / 100.0f;
-  utm.zone = nav_utm_zone0;
+  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
 
 #if !USE_BAROMETER
 #ifdef GPS_DT

diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c
@@ -273,20 +273,7 @@ void ins_float_invariant_init(void)
 void ins_reset_local_origin(void)
 {
 #if INS_FINV_USE_UTM
-  struct UtmCoor_f utm;
-  if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
-    utm.zone = gps.utm_pos.zone;
-    utm.east = gps.utm_pos.east / 100.0f;
-    utm.north = gps.utm_pos.north / 100.0f;
-  }
-  else {
-    /* Recompute UTM coordinates in this zone */
-    struct LlaCoor_f lla;
-    LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
-    struct UtmCoor_f utm;
-    utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
-    utm_of_lla_f(&utm, &lla);
-  }
+  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
   // ground_alt
   utm.alt = gps.hmsl  / 1000.0f;
   // reset state UTM ref
@@ -441,9 +428,10 @@ void ins_float_invariant_update_gps(struct GpsState *gps_s)
 
 #if INS_FINV_USE_UTM
     if (state.utm_initialized_f) {
+      struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
       // position (local ned)
-      ins_float_inv.meas.pos_gps.x = (gps_s->utm_pos.north / 100.0f) - state.utm_origin_f.north;
-      ins_float_inv.meas.pos_gps.y = (gps_s->utm_pos.east / 100.0f) - state.utm_origin_f.east;
+      ins_float_inv.meas.pos_gps.x = utm.north - state.utm_origin_f.north;
+      ins_float_inv.meas.pos_gps.y = utm.east - state.utm_origin_f.east;
       ins_float_inv.meas.pos_gps.z = state.utm_origin_f.alt - (gps_s->hmsl / 1000.0f);
       // speed
       ins_float_inv.meas.speed_gps.x = gps_s->ned_vel.x / 100.0f;

diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c
@@ -46,20 +46,7 @@ void ins_gps_utm_init(void)
 
 void ins_reset_local_origin(void)
 {
-  struct UtmCoor_f utm;
-
-  if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
-    utm.zone = gps.utm_pos.zone;
-    utm.east = gps.utm_pos.east / 100.0f;
-    utm.north = gps.utm_pos.north / 100.0f;
-  }
-  else {
-    /* Recompute UTM coordinates in this zone */
-    struct LlaCoor_f lla;
-    LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
-    utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
-    utm_of_lla_f(&utm, &lla);
-  }
+  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
 
   // ground_alt
   utm.alt = gps.hmsl / 1000.0f;
@@ -82,10 +69,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
                    uint32_t stamp __attribute__((unused)),
                    struct GpsState *gps_s)
 {
-  struct UtmCoor_f utm;
-  utm.east = gps_s->utm_pos.east / 100.0f;
-  utm.north = gps_s->utm_pos.north / 100.0f;
-  utm.zone = nav_utm_zone0;
+  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
   utm.alt = gps_s->hmsl / 1000.0f;
 
   // set position