[gps][ins] improve gps support for ins filters

- use conveniance functions to access GPS position and speed
- try to improve support of UBX fix status
- gps.h compatible with cpp files

conf/modules/ins_float_invariant.xml

@@ -50,6 +50,9 @@
     <define name="USE_AHRS_ALIGNER"/>
     <define name="INS_TYPE_H" value="modules/ins/ins_float_invariant_wrapper.h" type="string"/>
     <test firmware="rotorcraft"/>
+    <test firmware="fixedwing">
+      <define name="FIXEDWING_FIRMWARE"/>
+    </test>
   </makefile>
   <makefile target="sim">
     <define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_sim.h" type="string"/>

conf/modules/ins_gps_passthrough.xml

@@ -26,7 +26,7 @@
     <file name="ins.c"/>
     <file name="ins_gps_passthrough_utm.c"/>
   </makefile>
-  <makefile target="ap" firmware="rover">
+  <makefile target="ap|nps" firmware="rover">
     <define name="INS_TYPE_H" value="modules/ins/ins_gps_passthrough.h" type="string"/>
     <file name="ins.c"/>
     <file name="ins_gps_passthrough.c"/>

conf/modules/ins_mekf_wind.xml

@@ -82,6 +82,12 @@
     <define name="USE_AHRS_ALIGNER"/>
     <define name="INS_TYPE_H" value="modules/ins/ins_mekf_wind_wrapper.h" type="string"/>
     <test firmware="fixedwing">
+      <define name="FIXEDWING_FIRMWARE"/>
+      <define name="INS_TYPE_H" value="modules/ins/ins_mekf_wind_wrapper.h" type="string"/>
+      <include name="../ext/eigen"/>
+    </test>
+    <test firmware="rotorcraft">
+      <define name="ROTORCRAFT_FIRMWARE"/>
       <define name="INS_TYPE_H" value="modules/ins/ins_mekf_wind_wrapper.h" type="string"/>
       <include name="../ext/eigen"/>
     </test>

sw/airborne/math/pprz_geodetic_int.h

@@ -142,32 +142,37 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, st
 #define HIGH_RES_TRIG_FRAC  20
 
 #define VECT3_ENU_OF_NED(_o, _i) {    \
-    (_o).x = (_i).y;                    \
-    (_o).y = (_i).x;                    \
-    (_o).z = -(_i).z;                   \
+    (_o).x = (_i).y;                  \
+    (_o).y = (_i).x;                  \
+    (_o).z = -(_i).z;                 \
   }
 
 #define VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i)
 #define INT32_VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i)
 #define INT32_VECT3_ENU_OF_NED(_o, _i) VECT3_ENU_OF_NED(_o,_i)
 
-#define ECEF_BFP_OF_REAL(_o, _i) {          \
+#define VECT3_CM_OF_REAL(_o, _i) {          \
     (_o).x = (int32_t)CM_OF_M((_i).x);      \
     (_o).y = (int32_t)CM_OF_M((_i).y);      \
     (_o).z = (int32_t)CM_OF_M((_i).z);      \
   }
 
-#define ECEF_FLOAT_OF_BFP(_o, _i) {           \
-    (_o).x = M_OF_CM((float)(_i).x);          \
-    (_o).y = M_OF_CM((float)(_i).y);          \
-    (_o).z = M_OF_CM((float)(_i).z);          \
+#define VECT3_FLOAT_OF_CM(_o, _i) {         \
+    (_o).x = M_OF_CM((float)(_i).x);        \
+    (_o).y = M_OF_CM((float)(_i).y);        \
+    (_o).z = M_OF_CM((float)(_i).z);        \
   }
 
-#define ECEF_DOUBLE_OF_BFP(_o, _i) {          \
-    (_o).x = M_OF_CM((double)(_i).x);         \
-    (_o).y = M_OF_CM((double)(_i).y);         \
-    (_o).z = M_OF_CM((double)(_i).z);         \
+#define VECT3_DOUBLE_OF_CM(_o, _i) {        \
+    (_o).x = M_OF_CM((double)(_i).x);       \
+    (_o).y = M_OF_CM((double)(_i).y);       \
+    (_o).z = M_OF_CM((double)(_i).z);       \
   }
+
+#define ECEF_BFP_OF_REAL(_o, _i) VECT3_CM_OF_REAL(_o, _i)
+#define ECEF_FLOAT_OF_BFP(_o, _i) VECT3_FLOAT_OF_CM(_o, _i)
+#define ECEF_DOUBLE_OF_BFP(_o, _i) VECT3_DOUBLE_OF_CM(_o, _i)
+
 #define LLA_BFP_OF_REAL(_o, _i) {                \
     (_o).lat = (int32_t)EM7DEG_OF_RAD((_i).lat); \
     (_o).lon = (int32_t)EM7DEG_OF_RAD((_i).lon); \

sw/airborne/modules/gps/gps.c

@@ -404,6 +404,134 @@ void gps_parse_RTCM_INJECT(uint8_t *buf)
       DL_RTCM_INJECT_data(buf));
 }
 
+/**
+ * Get GPS lla (float)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return lla position in float (rad), altitude ellipsoid (m)
+ */
+struct LlaCoor_f lla_float_from_gps(struct GpsState *gps_s) {
+  struct LlaCoor_i lla_i = lla_int_from_gps(gps_s);
+  struct LlaCoor_f lla_f;
+  LLA_FLOAT_OF_BFP(lla_f, lla_i);
+  return lla_f;
+}
+
+/**
+ * Get GPS lla (integer)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return lla position (lat,lon: deg*1e7; alt: mm over ellipsoid)
+ */
+struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s) {
+  struct LlaCoor_i lla_i = { 0, 0, 0 };
+  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
+    return gps_s->lla_pos;
+  } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT)) {
+    lla_of_ecef_i(&lla_i, &gps_s->ecef_pos);
+  }
+  return lla_i;
+}
+
+/**
+ * Get GPS ecef pos (float)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return ecef position in float (m)
+ */
+struct EcefCoor_f ecef_float_from_gps(struct GpsState *gps_s) {
+  struct EcefCoor_i ecef_i = ecef_int_from_gps(gps_s);
+  struct EcefCoor_f ecef_f;
+  ECEF_FLOAT_OF_BFP(ecef_f, ecef_i);
+  return ecef_f;
+}
+
+/**
+ * Get GPS ecef pos (integer)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return ecef position in cm
+ */
+struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s) {
+  struct EcefCoor_i ecef_i = { 0, 0, 0 };
+  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT)) {
+    return gps_s->ecef_pos;
+  } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
+    ecef_of_lla_i(&ecef_i, &gps_s->lla_pos);
+  }
+  return ecef_i;
+}
+
+/**
+ * Get GPS ecef velocity (float)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return ecef velocity in float (m/s)
+ */
+struct EcefCoor_f ecef_vel_float_from_gps(struct GpsState *gps_s) {
+  struct EcefCoor_i ecef_vel_i = ecef_vel_int_from_gps(gps_s);
+  struct EcefCoor_f ecef_vel_f;
+  ECEF_FLOAT_OF_BFP(ecef_vel_f, ecef_vel_i);
+  return ecef_vel_f;
+}
+
+/**
+ * Get GPS ecef velocity (integer)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return ecef velocity in cm/s
+ */
+struct EcefCoor_i ecef_vel_int_from_gps(struct GpsState *gps_s) {
+  struct EcefCoor_i ecef_vel_i = { 0, 0, 0 };
+  if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) {
+    return gps_s->ecef_vel;
+  } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) {
+    struct LtpDef_i def;
+    if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
+      ltp_def_from_lla_i(&def, &gps_s->lla_pos);
+    } else { // assume ECEF
+      ltp_def_from_ecef_i(&def, &gps_s->ecef_pos);
+    }
+    ecef_of_ned_vect_i(&ecef_vel_i, &def, &gps_s->ned_vel);
+  }
+  return ecef_vel_i;
+}
+
+/**
+ * Get GPS ned velocity (float)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return ned velocity in float (m/s)
+ */
+struct NedCoor_f ned_vel_float_from_gps(struct GpsState *gps_s) {
+  struct NedCoor_i ned_vel_i = ned_vel_int_from_gps(gps_s);
+  struct NedCoor_f ned_vel_f;
+  VECT3_FLOAT_OF_CM(ned_vel_f, ned_vel_i);
+  return ned_vel_f;
+}
+
+/**
+ * Get GPS ned velocity (integer)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return ned velocity in cm/s
+ */
+struct NedCoor_i ned_vel_int_from_gps(struct GpsState *gps_s) {
+  struct NedCoor_i ned_vel_i = { 0, 0, 0 };
+  if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) {
+    return gps_s->ned_vel;
+  } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) {
+    struct LtpDef_i def;
+    if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
+      ltp_def_from_lla_i(&def, &gps_s->lla_pos);
+    } else { // assume ECEF
+      ltp_def_from_ecef_i(&def, &gps_s->ecef_pos);
+    }
+    ned_of_ecef_vect_i(&ned_vel_i, &def, &gps_s->ecef_vel);
+  }
+  return ned_vel_i;
+}
+
 /**
  * Convenience functions to get utm position from GPS state
  */

sw/airborne/modules/gps/gps.h

@@ -27,6 +27,9 @@
 #ifndef GPS_H
 #define GPS_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 #include "std.h"
 #include "math/pprz_geodetic_int.h"
@@ -207,6 +210,70 @@ extern struct GpsTimeSync gps_time_sync;
  */
 extern uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks);
 
+/**
+ * Get GPS lla (float)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return lla position in float (rad), altitude ellipsoid (m)
+ */
+extern struct LlaCoor_f lla_float_from_gps(struct GpsState *gps_s);
+
+/**
+ * Get GPS lla (integer)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return lla position (lat,lon: deg*1e7; alt: mm over ellipsoid)
+ */
+extern struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s);
+
+/**
+ * Get GPS ecef pos (float)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return ecef position in float (m)
+ */
+extern struct EcefCoor_f ecef_float_from_gps(struct GpsState *gps_s);
+
+/**
+ * Get GPS ecef pos (integer)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return ecef position in cm
+ */
+extern struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s);
+
+/**
+ * Get GPS ecef velocity (float)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return ecef velocity in float (m/s)
+ */
+extern struct EcefCoor_f ecef_vel_float_from_gps(struct GpsState *gps_s);
+
+/**
+ * Get GPS ecef velocity (integer)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return ecef velocity in cm/s
+ */
+extern struct EcefCoor_i ecef_vel_int_from_gps(struct GpsState *gps_s);
+
+/**
+ * Get GPS ned velocity (float)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return ned velocity in float (m/s)
+ */
+extern struct NedCoor_f ned_vel_float_from_gps(struct GpsState *gps_s);
+
+/**
+ * Get GPS ned velocity (integer)
+ * Converted on the fly if not available
+ * @param[in] gps_s pointer to the gps structure
+ * @return ned velocity in cm/s
+ */
+extern struct NedCoor_i ned_vel_int_from_gps(struct GpsState *gps_s);
+
 /**
  * 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
@@ -243,4 +310,8 @@ extern uint16_t gps_day_number(uint16_t year, uint8_t month, uint8_t day);
  */
 extern uint16_t gps_week_number(uint16_t year, uint8_t month, uint8_t day);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif /* GPS_H */

sw/airborne/modules/gps/gps_ubx.c

@@ -194,14 +194,20 @@ static void gps_ubx_parse_nav_pvt(void)
   gps_ubx.state.hacc        = UBX_NAV_PVT_hAcc(gps_ubx.msg_buf) / 10;
   gps_ubx.state.vacc        = UBX_NAV_PVT_vAcc(gps_ubx.msg_buf) / 10;
   gps_ubx.state.sacc        = UBX_NAV_PVT_sAcc(gps_ubx.msg_buf) / 10;
+
+  //FIXME stupid workaround for PVT only
+  gps_ubx.state.pacc = gps_ubx.state.hacc; // report horizontal accuracy
 }
 
 static void gps_ubx_parse_nav_sol(void)
 {
   // Copy time and fix information
-#if !USE_GPS_UBX_RTCM
-  gps_ubx.state.fix        = UBX_NAV_SOL_gpsFix(gps_ubx.msg_buf);
-#endif
+  uint8_t fix = UBX_NAV_SOL_gpsFix(gps_ubx.msg_buf);
+  if ((fix == GPS_FIX_3D && fix > gps_ubx.state.fix) || fix < GPS_FIX_3D) {
+    // update only if fix is better than current or fix not 3D
+    // leaving fix if in GNSS or RTK mode
+    gps_ubx.state.fix = fix;
+  }
   gps_ubx.state.tow        = UBX_NAV_SOL_iTOW(gps_ubx.msg_buf);
   gps_ubx.state.week       = UBX_NAV_SOL_week(gps_ubx.msg_buf);
   gps_ubx.state.num_sv     = UBX_NAV_SOL_numSV(gps_ubx.msg_buf);
@@ -351,9 +357,12 @@ static void gps_ubx_parse_nav_sat(void)
 
 static void gps_ubx_parse_nav_status(void)
 {
-#if !USE_GPS_UBX_RTCM
-  gps_ubx.state.fix     = UBX_NAV_STATUS_gpsFix(gps_ubx.msg_buf);
-#endif
+  uint8_t fix = UBX_NAV_STATUS_gpsFix(gps_ubx.msg_buf);
+  if ((fix == GPS_FIX_3D && fix > gps_ubx.state.fix) || fix < GPS_FIX_3D) {
+    // update only if fix is better than current or fix not 3D
+    // leaving fix if in GNSS or RTK mode
+    gps_ubx.state.fix = fix;
+  }
   gps_ubx.state.tow     = UBX_NAV_STATUS_iTOW(gps_ubx.msg_buf);
   gps_ubx.status_flags  = UBX_NAV_STATUS_flags(gps_ubx.msg_buf);
 }
@@ -370,7 +379,6 @@ static void gps_ubx_parse_nav_relposned(void)
     uint8_t gnssFixOK   = RTCMgetbitu(&flags, 7, 1);
 
     /* Only save the latest valid relative position */
-    if(relPosValid) {
       if (diffSoln && carrSoln == 2) {
         gps_ubx.state.fix = 5; // rtk
       } else if(diffSoln && carrSoln == 1) {
@@ -380,6 +388,7 @@ static void gps_ubx_parse_nav_relposned(void)
       } else{
         gps_ubx.state.fix = 0;
       }
+    if(relPosValid) {
 
       gps_relposned.iTOW          = UBX_NAV_RELPOSNED_iTOW(gps_ubx.msg_buf);
       gps_relposned.refStationId  = UBX_NAV_RELPOSNED_refStationId(gps_ubx.msg_buf);

sw/airborne/modules/ins/ins.c

@@ -64,6 +64,8 @@ void WEAK ins_reset_local_origin(void)
 
 void WEAK ins_reset_altitude_ref(void) {}
 
+void WEAK ins_reset_vertical_pos(void) {}
+
 #if USE_GPS
 void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm)
 {

sw/airborne/modules/ins/ins.h

@@ -50,6 +50,12 @@ extern void ins_reset_local_origin(void);
  */
 extern void ins_reset_altitude_ref(void);
 
+/** INS vertical position reset.
+ *  Reset only vertical position to zero.
+ *  Does nothing if not implemented by specific INS algorithm.
+ */
+extern void ins_reset_vertical_pos(void);
+
 /** INS utm zone reset.
  *  Reset UTM zone according the the actual position.
  *  Only used with fixedwing firmware.

sw/airborne/modules/ins/ins_alt_float.c

@@ -210,6 +210,7 @@ void ins_alt_float_update_gps(struct GpsState *gps_s __attribute__((unused)))
   }
 
   struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
+  struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s);
 
 #if !USE_BAROMETER
 #ifdef GPS_DT
@@ -234,18 +235,15 @@ void ins_alt_float_update_gps(struct GpsState *gps_s __attribute__((unused)))
     alt_kalman_reset();
   } else {
     alt_kalman(utm.alt, dt);
-    ins_altf.alt_dot = -gps_s->ned_vel.z / 100.0f;
+    ins_altf.alt_dot = -ned_vel.z;
   }
-#endif
+#endif // !USE_BAROMETER
+
   utm.alt = ins_altf.alt;
   // set position
   stateSetPositionUtm_f(&utm);
 
-  struct NedCoor_f ned_vel = {
-    gps_s->ned_vel.x / 100.0f,
-    gps_s->ned_vel.y / 100.0f,
-    -ins_altf.alt_dot
-  };
+  ned_vel.z = -ins_altf.alt_dot; // vz (down) from filter
   // set velocity
   stateSetSpeedNed_f(&ned_vel);
 

sw/airborne/modules/ins/ins_ekf2.cpp

@@ -854,8 +854,9 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
 {
   gps_message gps_msg = {};
   gps_msg.time_usec = stamp;
-  gps_msg.lat = gps_s->lla_pos.lat;
-  gps_msg.lon = gps_s->lla_pos.lon;
+  struct LlaCoor_i lla_pos = lla_int_from_gps(gps_s);
+  gps_msg.lat = lla_pos.lat;
+  gps_msg.lon = lla_pos.lon;
   gps_msg.alt = gps_s->hmsl;
 #if INS_EKF2_GPS_COURSE_YAW
   gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7);
@@ -869,9 +870,10 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
   gps_msg.epv = gps_s->vacc / 100.0;
   gps_msg.sacc = gps_s->sacc / 100.0;
   gps_msg.vel_m_s = gps_s->gspeed / 100.0;
-  gps_msg.vel_ned(0) = (gps_s->ned_vel.x) / 100.0;
-  gps_msg.vel_ned(1) = (gps_s->ned_vel.y) / 100.0;
-  gps_msg.vel_ned(2) = (gps_s->ned_vel.z) / 100.0;
+  struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s);
+  gps_msg.vel_ned(0) = ned_vel.x;
+  gps_msg.vel_ned(1) = ned_vel.y;
+  gps_msg.vel_ned(2) = ned_vel.z;
   gps_msg.vel_ned_valid = bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT);
   gps_msg.nsats = gps_s->num_sv;
   gps_msg.pdop = gps_s->pdop;

sw/airborne/modules/ins/ins_float_invariant.c

@@ -286,8 +286,11 @@ void ins_reset_local_origin(void)
   // reset state UTM ref
   stateSetLocalUtmOrigin_f(&utm);
 #else
+  struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps);
+  struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
   struct LtpDef_i ltp_def;
-  ltp_def_from_ecef_i(&ltp_def, &gps.ecef_pos);
+  ltp_def_from_ecef_i(&ltp_def, &ecef_pos);
+  ltp_def.lla.alt = lla_pos.alt;
   ltp_def.hmsl = gps.hmsl;
   stateSetLocalOrigin_i(&ltp_def);
 #endif
@@ -453,18 +456,18 @@ void ins_float_invariant_update_gps(struct GpsState *gps_s)
       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 - utm.alt;
       // speed
-      ins_float_inv.meas.speed_gps.x = gps_s->ned_vel.x / 100.0f;
-      ins_float_inv.meas.speed_gps.y = gps_s->ned_vel.y / 100.0f;
-      ins_float_inv.meas.speed_gps.z = gps_s->ned_vel.z / 100.0f;
+      ins_float_inv.meas.speed_gps = ned_vel_float_from_gps(gps_s);
     }
 #else
     if (state.ned_initialized_f) {
+      // position
       struct NedCoor_i gps_pos_cm_ned, ned_pos;
-      ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &gps_s->ecef_pos);
+      struct EcefCoor_i ecef_pos_i = ecef_int_from_gps(gps_s);
+      ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &ecef_pos_i);
       INT32_VECT3_SCALE_2(ned_pos, gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
       NED_FLOAT_OF_BFP(ins_float_inv.meas.pos_gps, ned_pos);
-      struct EcefCoor_f ecef_vel;
-      ECEF_FLOAT_OF_BFP(ecef_vel, gps_s->ecef_vel);
+      // speed
+      struct EcefCoor_f ecef_vel = ecef_vel_float_from_gps(gps_s);
       ned_of_ecef_vect_f(&ins_float_inv.meas.speed_gps, &state.ned_origin_f, &ecef_vel);
     }
 #endif

sw/airborne/modules/ins/ins_gps_passthrough.c

@@ -89,13 +89,15 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
 
   /* simply scale and copy pos/speed from gps */
   struct NedCoor_i gps_pos_cm_ned;
-  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &gps_s->ecef_pos);
+  struct EcefCoor_i ecef_pos = ecef_int_from_gps(gps_s);
+  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &ecef_pos);
   INT32_VECT3_SCALE_2(ins_gp.ltp_pos, gps_pos_cm_ned,
                       INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
   stateSetPositionNed_i(&ins_gp.ltp_pos);
 
   struct NedCoor_i gps_speed_cm_s_ned;
-  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &gps_s->ecef_vel);
+  struct EcefCoor_i ecef_vel = ecef_vel_int_from_gps(gps_s);
+  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &ecef_vel);
   INT32_VECT3_SCALE_2(ins_gp.ltp_speed, gps_speed_cm_s_ned,
                       INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
   stateSetSpeedNed_i(&ins_gp.ltp_speed);
@@ -171,8 +173,10 @@ void ins_gps_passthrough_init(void)
 
 void ins_reset_local_origin(void)
 {
-  ltp_def_from_ecef_i(&ins_gp.ltp_def, &gps.ecef_pos);
-  ins_gp.ltp_def.lla.alt = gps.lla_pos.alt;
+  struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps);
+  struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
+  ltp_def_from_ecef_i(&ins_gp.ltp_def, &ecef_pos);
+  ins_gp.ltp_def.lla.alt = lla_pos.alt;
   ins_gp.ltp_def.hmsl = gps.hmsl;
   stateSetLocalOrigin_i(&ins_gp.ltp_def);
   ins_gp.ltp_initialized = true;

sw/airborne/modules/ins/ins_gps_passthrough_utm.c

@@ -56,11 +56,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
   // set position
   stateSetPositionUtm_f(&utm);
 
-  struct NedCoor_f ned_vel = {
-    gps_s->ned_vel.x / 100.0f,
-    gps_s->ned_vel.y / 100.0f,
-    gps_s->ned_vel.z / 100.0f
-  };
+  struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s);
   // set velocity
   stateSetSpeedNed_f(&ned_vel);
 }

sw/airborne/modules/ins/ins_int.c

@@ -250,8 +250,10 @@ void ins_reset_local_origin(void)
 {
 #if USE_GPS
   if (GpsFixValid()) {
-    ltp_def_from_ecef_i(&ins_int.ltp_def, &gps.ecef_pos);
-    ins_int.ltp_def.lla.alt = gps.lla_pos.alt;
+    struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps);
+    struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
+    ltp_def_from_ecef_i(&ins_int.ltp_def, &ecef_pos);
+    ins_int.ltp_def.lla.alt = lla_pos.alt;
     ins_int.ltp_def.hmsl = gps.hmsl;
     ins_int.ltp_initialized = true;
     stateSetLocalOrigin_i(&ins_int.ltp_def);
@@ -272,10 +274,11 @@ void ins_reset_altitude_ref(void)
 {
 #if USE_GPS
   if (GpsFixValid()) {
+    struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
     struct LlaCoor_i lla = {
       .lat = state.ned_origin_i.lla.lat,
       .lon = state.ned_origin_i.lla.lon,
-      .alt = gps.lla_pos.alt
+      .alt = lla_pos.alt
     };
     ltp_def_from_lla_i(&ins_int.ltp_def, &lla);
     ins_int.ltp_def.hmsl = gps.hmsl;
@@ -285,6 +288,11 @@ void ins_reset_altitude_ref(void)
   ins_int.vf_reset = true;
 }
 
+void ins_reset_vertical_pos(void)
+{
+  ins_int.vf_reset = true;
+}
+
 void ins_int_propagate(struct Int32Vect3 *accel, float dt)
 {
   /* untilt accels */
@@ -396,7 +404,8 @@ void ins_int_update_gps(struct GpsState *gps_s)
   }
 
   struct NedCoor_i gps_pos_cm_ned;
-  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &gps_s->ecef_pos);
+  struct EcefCoor_i ecef_pos_i = ecef_int_from_gps(gps_s);
+  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &ecef_pos_i);
 
   /* calculate body frame position taking BODY_TO_GPS translation (in cm) into account */
 #ifdef INS_BODY_TO_GPS_X
@@ -417,7 +426,8 @@ void ins_int_update_gps(struct GpsState *gps_s)
 
   /// @todo maybe use gps_s->ned_vel directly??
   struct NedCoor_i gps_speed_cm_s_ned;
-  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &gps_s->ecef_vel);
+  struct EcefCoor_i ecef_vel_i = ecef_vel_int_from_gps(gps_s);
+  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &ecef_vel_i);
 
 #if INS_USE_GPS_ALT
   vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS);

sw/airborne/modules/ins/ins_mekf_wind_wrapper.c

@@ -39,8 +39,7 @@
 #include "generated/airframe.h"
 #include "generated/flight_plan.h"
 
-#define MEKF_WIND_USE_UTM TRUE
-#if MEKF_WIND_USE_UTM
+#if FIXEDWING_FIRMWARE
 #include "firmwares/fixedwing/nav.h"
 #endif
 
@@ -441,24 +440,22 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
 {
 	if (ins_mekf_wind.is_aligned && gps_s->fix >= GPS_FIX_3D) {
 
-#if MEKF_WIND_USE_UTM
+#if FIXEDWING_FIRMWARE
 		if (state.utm_initialized_f) {
 			struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
-      struct FloatVect3 pos, speed;
+      struct NedCoor_f pos, speed;
 			// position (local ned)
 			pos.x = utm.north - state.utm_origin_f.north;
 			pos.y = utm.east - state.utm_origin_f.east;
 			pos.z = state.utm_origin_f.alt - utm.alt;
 			// speed
-			speed.x = gps_s->ned_vel.x / 100.0f;
-			speed.y = gps_s->ned_vel.y / 100.0f;
-			speed.z = gps_s->ned_vel.z / 100.0f;
+			speed = ned_vel_float_from_gps(gps_s);
       if (!ins_mekf_wind.gps_initialized) {
-        ins_mekf_wind_set_pos_ned((struct NedCoor_f*)(&pos));
-        ins_mekf_wind_set_speed_ned((struct NedCoor_f*)(&speed));
+        ins_mekf_wind_set_pos_ned(&pos);
+        ins_mekf_wind_set_speed_ned(&speed);
         ins_mekf_wind.gps_initialized = true;
       }
-      ins_mekf_wind_update_pos_speed(&pos, &speed);
+      ins_mekf_wind_update_pos_speed((struct FloatVect3*)(&pos), (struct FloatVect3*)(&speed));
 
 #if LOG_MEKFW_FILTER
       if (LogFileIsOpen()) {
@@ -472,15 +469,15 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
 
 #else
 		if (state.ned_initialized_f) {
-      struct FloatVect3 pos, speed;
+      struct NedCoor_f pos, speed;
 			struct NedCoor_i gps_pos_cm_ned, ned_pos;
-			ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &gps_s->ecef_pos);
+      struct EcefCoor_i ecef_pos_i = ecef_int_from_gps(gps_s);
+			ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &ecef_pos_i);
 			INT32_VECT3_SCALE_2(ned_pos, gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
 			NED_FLOAT_OF_BFP(pos, ned_pos);
-			struct EcefCoor_f ecef_vel;
-			ECEF_FLOAT_OF_BFP(ecef_vel, gps_s->ecef_vel);
+      struct EcefCoor_f ecef_vel = ecef_vel_float_from_gps(gps_s);
 			ned_of_ecef_vect_f(&speed, &state.ned_origin_f, &ecef_vel);
-      ins_mekf_wind_update_pos_speed(&pos, &speed);
+      ins_mekf_wind_update_pos_speed((struct FloatVect3*)(&pos), (struct FloatVect3*)(&speed));
 
 #if LOG_MEKFW_FILTER
       if (LogFileIsOpen()) {
@@ -522,7 +519,7 @@ static void set_state_from_ins(void)
 void ins_mekf_wind_wrapper_init(void)
 {
   // init position
-#if MEKF_WIND_USE_UTM
+#if FIXEDWING_FIRMWARE
   struct UtmCoor_f utm0;
   utm0.north = (float)nav_utm_north0;
   utm0.east = (float)nav_utm_east0;
@@ -606,13 +603,16 @@ void ins_mekf_wind_wrapper_init(void)
 
 void ins_reset_local_origin(void)
 {
-#if MEKF_WIND_USE_UTM
+#if FIXEDWING_FIRMWARE
   struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
   // reset state UTM ref
   stateSetLocalUtmOrigin_f(&utm);
 #else
+  struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps);
+  struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
   struct LtpDef_i ltp_def;
-  ltp_def_from_ecef_i(&ltp_def, &gps.ecef_pos);
+  ltp_def_from_ecef_i(&ltp_def, &ecef_pos);
+  ltp_def.lla.alt = lla_pos.alt;
   ltp_def.hmsl = gps.hmsl;
   stateSetLocalOrigin_i(&ltp_def);
 #endif
@@ -622,7 +622,7 @@ void ins_reset_local_origin(void)
 
 void ins_reset_altitude_ref(void)
 {
-#if MEKF_WIND_USE_UTM
+#if FIXEDWING_FIRMWARE
   struct UtmCoor_f utm = state.utm_origin_f;
   utm.alt = gps.hmsl / 1000.0f;
   stateSetLocalUtmOrigin_f(&utm);

sw/airborne/modules/ins/ins_xsens.c

@@ -94,11 +94,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
   // set position
   stateSetPositionUtm_f(&utm);
 
-  struct NedCoor_f ned_vel = {
-    gps_s->ned_vel.x / 100.,
-    gps_s->ned_vel.y / 100.,
-    gps_s->ned_vel.z / 100.
-  };
+  struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s);
   // set velocity
   stateSetSpeedNed_f(&ned_vel);
 }

sw/airborne/modules/ins/ins_xsens700.c

@@ -96,11 +96,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
   // set position
   stateSetPositionUtm_f(&utm);
 
-  struct NedCoor_f ned_vel = {
-    gps_s->ned_vel.x / 100.,
-    gps_s->ned_vel.y / 100.,
-    gps_s->ned_vel.z / 100.
-  };
+  struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s);
   // set velocity
   stateSetSpeedNed_f(&ned_vel);
 }