[ins] reset reference position from INS not from NAV

Nav is (almost) independent of the GPS which is a sensor and should only
be seen by the INS/AHRS subsystems.
It also allow to make shared INS filters more easily (example with
invariant filter).

conf/flight_plans/versatile.xml

@@ -12,8 +12,8 @@
     <waypoint name="MOB" x="-11.5" y="-21.2"/>
     <waypoint name="S1" x="-151.6" y="80.4"/>
     <waypoint name="S2" x="180.1" y="214.9"/>
-    <waypoint alt="30" name="AF" x="200" y="-10"/>
-    <waypoint alt="0" name="TD" x="80.0" y="20.0"/>
+    <waypoint alt="215.0" name="AF" x="200" y="-10"/>
+    <waypoint alt="185.0" name="TD" x="80.0" y="20.0"/>
     <waypoint name="BASELEG" x="26.9" y="-23.0"/>
     <waypoint name="_1" x="-100" y="0"/>
     <waypoint name="_2" x="-100" y="200"/>

sw/airborne/firmwares/rotorcraft/navigation.c

@@ -307,21 +307,12 @@ static inline void nav_set_altitude( void ) {
 
 /** Reset the geographic reference to the current GPS fix */
 unit_t nav_reset_reference( void ) {
-  ins_impl.ltp_initialized = FALSE;
-  ins.hf_realign = TRUE;
-  ins.vf_realign = TRUE;
+  ins_reset_ground_ref();
   return 0;
 }
 
 unit_t nav_reset_alt( void ) {
-  ins.vf_realign = TRUE;
-
-#if USE_GPS
-  ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
-  ins_impl.ltp_def.hmsl = gps.hmsl;
-  stateSetLocalOrigin_i(&ins_impl.ltp_def);
-#endif
-
+  ins_reset_altitude_ref();
   return 0;
 }
 
@@ -342,7 +333,7 @@ void nav_periodic_task() {
   /* run carrot loop */
   nav_run();
 
-  ground_alt = POS_BFP_OF_REAL((float)ins_impl.ltp_def.hmsl / 1000.);
+  ground_alt = POS_BFP_OF_REAL(state.ned_origin_f.hmsl);
 }
 
 void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos) {

sw/airborne/subsystems/ins.h

@@ -60,6 +60,26 @@ extern void ins_init( void );
  */
 extern void ins_periodic( void );
 
+/** INS ground reference reset.
+ *  Reset horizontal and vertical reference to the current position.
+ *  Needs to be implemented by each INS algorithm.
+ */
+extern void ins_reset_ground_ref( void );
+
+/** INS altitude reference reset.
+ *  Reset only vertical reference to the current altitude.
+ *  Needs to be implemented by each INS algorithm.
+ */
+extern void ins_reset_altitude_ref( void );
+
+/** INS utm zone reset.
+ *  Reset UTM zone according te the actual position.
+ *  Needs to be implemented by each INS algorithm but is only
+ *  used with fixedwing firmware.
+ *  @param utm initial utm zone, returns the corrected utm position
+ */
+extern void ins_reset_utm_zone(struct UtmCoor_f * utm);
+
 /** INS horizontal realign.
  *  @param pos new horizontal position to set
  *  @param speed new horizontal speed to set

sw/airborne/subsystems/ins/ins_alt_float.c

@@ -87,6 +87,7 @@ void ins_init() {
   AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
 #endif
   ins.vf_realign = FALSE;
+  ins.hf_realign = FALSE;
 
   alt_kalman(0.);
 
@@ -96,6 +97,54 @@ void ins_init() {
 void ins_periodic( void ) {
 }
 
+/** Reset the geographic reference to the current GPS fix */
+void ins_reset_ground_ref( void ) {
+  struct UtmCoor_f utm;
+#ifdef GPS_USE_LATLONG
+  /* Recompute UTM coordinates in this zone */
+  struct LlaCoor_f lla;
+  lla.lat = gps.lla_pos.lat/1e7;
+  lla.lon = gps.lla_pos.lon/1e7;
+  utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
+  utm_of_lla_f(&utm, &lla);
+#else
+  utm.zone = gps.utm_pos.zone;
+  utm.east = gps.utm_pos.east/100;
+  utm.north = gps.utm_pos.north/100;
+#endif
+  // ground_alt
+  utm.alt = gps.hmsl/1000.;
+
+  // reset state UTM ref
+  stateSetLocalUtmOrigin_f(&utm);
+
+  // reset filter flag
+  ins.vf_realign = TRUE;
+}
+
+void ins_reset_altitude_ref( void ) {
+  struct UtmCoor_f utm = state.utm_origin_f;
+  // ground_alt
+  utm.alt = gps.hmsl/1000.;
+  // reset state UTM ref
+  stateSetLocalUtmOrigin_f(&utm);
+  // reset filter flag
+  ins.vf_realign = TRUE;
+}
+
+void ins_reset_utm_zone(struct UtmCoor_f * utm) {
+  struct LlaCoor_f lla0;
+  lla_of_utm_f(&lla0, utm);
+#ifdef GPS_USE_LATLONG
+  utm->zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
+#else
+  utm->zone = gps.utm_pos.zone;
+#endif
+  utm_of_lla_f(utm, &lla0);
+
+  stateSetLocalUtmOrigin_f(utm);
+}
+
 void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {
 }
 

sw/airborne/subsystems/ins/ins_alt_float.h

@@ -42,6 +42,14 @@ extern float ins_baro_alt;
 extern bool_t ins_baro_initialized;
 #endif
 
+/** Reset the UTM zone to the current gps fix
+ *
+ * This function must be called with a valid GPS position
+ *
+ * @param utm initial utm position, returned with a corrected utm zone
+ */
+extern void ins_reset_utm_zone(struct UtmCoor_f * utm);
+
 extern void alt_kalman_reset( void );
 extern void alt_kalman_init( void );
 extern void alt_kalman( float );

sw/airborne/subsystems/ins/ins_ardrone2.c

@@ -76,6 +76,18 @@ void ins_periodic( void ) {
     ins.status = INS_RUNNING;
 }
 
+void ins_reset_ground_ref( void ) {
+  ins_impl.ltp_initialized = FALSE;
+}
+
+void ins_reset_altitude_ref( void ) {
+#if USE_GPS
+  ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
+  ins_impl.ltp_def.hmsl = gps.hmsl;
+  stateSetLocalOrigin_i(&ins_impl.ltp_def);
+#endif
+}
+
 void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {
 
 }

sw/airborne/subsystems/ins/ins_float_invariant.c

@@ -58,6 +58,20 @@
 #include "messages.h"
 #include "subsystems/datalink/downlink.h"
 
+#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+static void send_ins_ref(void) {
+  float foo = 0.;
+  if (state.ned_initialized_i) {
+    DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice,
+        &state.ned_origin_i.ecef.x, &state.ned_origin_i.ecef.y, &state.ned_origin_i.ecef.z,
+        &state.ned_origin_i.lla.lat, &state.ned_origin_i.lla.lon, &state.ned_origin_i.lla.alt,
+        &state.ned_origin_i.hmsl, &foo);
+  }
+}
+#endif
+
+
 #if LOG_INVARIANT_FILTER
 #include "sdLog.h"
 #include "subsystems/chibios-libopencm3/chibios_sdlog.h"
@@ -221,7 +235,7 @@ void ins_init() {
   ecef_of_lla_i(&ecef_nav0, &llh_nav0);
   struct LtpDef_i ltp_def;
   ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
-  ins_impl.ltp_def.hmsl = NAV_ALT0;
+  ltp_def.hmsl = NAV_ALT0;
   stateSetLocalOrigin_i(&ltp_def);
 #endif
 
@@ -250,9 +264,76 @@ void ins_init() {
   ins.hf_realign = FALSE;
   ins.vf_realign = FALSE;
 
+#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
+#endif
 }
 
 void ins_periodic(void) {}
+void ins_propagate(void) {}
+
+
+void ins_reset_ground_ref( void ) {
+#if INS_UPDATE_FW_ESTIMATOR
+  struct UtmCoor_f utm;
+#ifdef GPS_USE_LATLONG
+  /* Recompute UTM coordinates in this zone */
+  struct LlaCoor_f lla;
+  lla.lat = gps.lla_pos.lat/1e7;
+  lla.lon = gps.lla_pos.lon/1e7;
+  utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
+  utm_of_lla_f(&utm, &lla);
+#else
+  utm.zone = gps.utm_pos.zone;
+  utm.east = gps.utm_pos.east/100;
+  utm.north = gps.utm_pos.north/100;
+#endif
+  // ground_alt
+  utm.alt = gps.hmsl/1000.;
+  // reset state UTM ref
+  stateSetLocalUtmOrigin_f(&utm);
+#else
+  struct LtpDef_i ltp_def;
+  ltp_def_from_ecef_i(&ltp_def, &gps.ecef_pos);
+  ltp_def.hmsl = gps.hmsl;
+  stateSetLocalOrigin_i(&ltp_def);
+#endif
+  ins.hf_realign = FALSE;
+  ins.vf_realign = FALSE;
+}
+
+void ins_reset_altitude_ref( void ) {
+#if INS_UPDATE_FW_ESTIMATOR
+  struct UtmCoor_f utm = state.utm_origin_f;
+  utm.alt = gps.hmsl/1000.;
+  stateSetLocalUtmOrigin_f(&utm);
+#else
+  struct LtpDef_i ltp_def = state.ned_origin_i;
+  ltp_def.lla.alt = gps.lla_pos.alt;
+  ltp_def.hmsl = gps.hmsl;
+  stateSetLocalOrigin_i(&ltp_def);
+#endif
+  ins.vf_realign = FALSE;
+}
+
+#if INS_UPDATE_FW_ESTIMATOR
+void ins_reset_utm_zone(struct UtmCoor_f * utm) {
+  struct LlaCoor_f lla0;
+  lla_of_utm_f(&lla0, utm);
+#ifdef GPS_USE_LATLONG
+  utm->zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
+#else
+  utm->zone = gps.utm_pos.zone;
+#endif
+  utm_of_lla_f(utm, &lla0);
+
+  stateSetLocalUtmOrigin_f(utm);
+}
+#endif
+
+void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {}
+void ins_realign_v(float z __attribute__ ((unused))) {}
+
 
 void ahrs_init(void) {
   ahrs.status = AHRS_UNINIT;
@@ -407,10 +488,13 @@ void ahrs_update_gps(void) {
 #else
     if (state.ned_initialized_f) {
       struct NedCoor_f gps_pos_cm_ned;
-      ned_of_ecef_point_f(&gps_pos_cm_ned, &state.ned_origin_f, &gps.ecef_pos);
-      VECT3_SDIV(ins_impl.meas.pos_gps, gps_pos_m_ned, 100.);
+      struct EcefCoor_f ecef_pos, ecef_vel;
+      ECEF_FLOAT_OF_BFP(ecef_pos, gps.ecef_pos);
+      ned_of_ecef_point_f(&gps_pos_cm_ned, &state.ned_origin_f, &ecef_pos);
+      VECT3_SDIV(ins_impl.meas.pos_gps, gps_pos_cm_ned, 100.);
       struct NedCoor_f gps_speed_cm_s_ned;
-      ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &state.ned_origin_f, &gps.ecef_vel);
+      ECEF_FLOAT_OF_BFP(ecef_vel, gps.ecef_vel);
+      ned_of_ecef_vect_f(&gps_speed_cm_s_ned, &state.ned_origin_f, &ecef_vel);
       VECT3_SDIV(ins_impl.meas.speed_gps, gps_speed_cm_s_ned, 100.);
     }
 #endif
@@ -469,8 +553,8 @@ void ahrs_update_mag(void) {
  */
 static inline void invariant_model(float * o, const float * x, const int n, const float * u, const int m __attribute__((unused))) {
 
-  const struct inv_state * s = (struct inv_state *)x;
-  const struct inv_command * c = (struct inv_command *)u;
+  const struct inv_state * s = (const struct inv_state *)x;
+  const struct inv_command * c = (const struct inv_command *)u;
   struct inv_state s_dot;
   struct FloatRates rates;
   struct FloatVect3 tmp_vect;

sw/airborne/subsystems/ins/ins_gps_passthrough.c

@@ -45,6 +45,35 @@ void ins_init() {
 void ins_periodic( void ) {
 }
 
+void ins_reset_ground_ref( void ) {
+  struct UtmCoor_f utm;
+#ifdef GPS_USE_LATLONG
+  /* Recompute UTM coordinates in this zone */
+  struct LlaCoor_f lla;
+  lla.lat = gps.lla_pos.lat/1e7;
+  lla.lon = gps.lla_pos.lon/1e7;
+  utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
+  utm_of_lla_f(&utm, &lla);
+#else
+  utm.zone = gps.utm_pos.zone;
+  utm.east = gps.utm_pos.east/100;
+  utm.north = gps.utm_pos.north/100;
+#endif
+  // ground_alt
+  utm.alt = gps.hmsl/1000.;
+  // reset state UTM ref
+  stateSetLocalUtmOrigin_f(&utm);
+}
+
+void ins_reset_altitude_ref( void ) {
+  struct UtmCoor_f utm = state.utm_origin_f;
+  utm.alt = gps.hmsl/1000.;
+  stateSetLocalUtmOrigin_f(&utm);
+}
+
+void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {}
+void ins_realign_v(float z __attribute__ ((unused))) {}
+
 void ins_update_gps(void) {
   struct UtmCoor_f utm;
   utm.east = gps.utm_pos.east / 100.;

sw/airborne/subsystems/ins/ins_int.c

@@ -170,6 +170,23 @@ void ins_periodic(void) {
     ins.status = INS_RUNNING;
 }
 
+void ins_reset_ground_ref( void ) {
+  ins_impl.ltp_initialized = FALSE;
+#if USE_HFF
+  ins.hf_realign = TRUE;
+#endif
+  ins.vf_realign = TRUE;
+}
+
+void ins_reset_altitude_ref( void ) {
+#if USE_GPS
+  ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
+  ins_impl.ltp_def.hmsl = gps.hmsl;
+  stateSetLocalOrigin_i(&ins_impl.ltp_def);
+#endif
+  ins.vf_realign = TRUE;
+}
+
 #if USE_HFF
 void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
   b2_hff_realign(pos, speed);