[ins] cleaning and dox update to better match what it actually does

sw/airborne/firmwares/rotorcraft/navigation.c

@@ -31,7 +31,7 @@
 #include "firmwares/rotorcraft/navigation.h"
 
 #include "pprz_debug.h"
-#include "subsystems/gps.h"
+#include "subsystems/gps.h" // needed by auto_nav from the flight plan
 #include "subsystems/ins.h"
 #include "state.h"
 
@@ -307,7 +307,7 @@ static inline void nav_set_altitude( void ) {
 
 /** Reset the geographic reference to the current GPS fix */
 unit_t nav_reset_reference( void ) {
-  ins_reset_ground_ref();
+  ins_reset_local_origin();
   return 0;
 }
 

sw/airborne/subsystems/ins.h

@@ -42,9 +42,9 @@
 
 /** Inertial Navigation System state */
 struct Ins {
-  uint8_t status; ///< status of the INS
+  uint8_t status;     ///< status of the INS
-  bool_t hf_realign; ///< realign horizontally if true
+  bool_t hf_realign;  ///< flag to request to realign horizontal filter to current position (local origin unchanged)
-  bool_t vf_realign; ///< realign vertically if true
+  bool_t vf_realign;  ///< flag to request to realign vertical filter to ground level (local origin unchanged)
 };
 
 /** global INS state */
@@ -60,11 +60,11 @@ extern void ins_init( void );
  */
 extern void ins_periodic( void );
 
-/** INS ground reference reset.
+/** INS local origin 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 );
+extern void ins_reset_local_origin( void );
 
 /** INS altitude reference reset.
  *  Reset only vertical reference to the current altitude.
@@ -81,6 +81,7 @@ extern void ins_reset_altitude_ref( void );
 extern void ins_reset_utm_zone(struct UtmCoor_f * utm);
 
 /** INS horizontal realign.
+ *  This only reset the filter to a given value, but doesn't change the local reference.
  *  @param pos new horizontal position to set
  *  @param speed new horizontal speed to set
  *  Needs to be implemented by each INS algorithm.
@@ -88,6 +89,7 @@ extern void ins_reset_utm_zone(struct UtmCoor_f * utm);
 extern void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed);
 
 /** INS vertical realign.
+ *  This only reset the filter to a given value, but doesn't change the local reference.
  *  @param z new altitude to set
  *  Needs to be implemented by each INS algorithm.
  */

sw/airborne/subsystems/ins/ins_alt_float.c

@@ -98,7 +98,7 @@ void ins_periodic( void ) {
 }
 
 /** Reset the geographic reference to the current GPS fix */
-void ins_reset_ground_ref( void ) {
+void ins_reset_local_origin( void ) {
   struct UtmCoor_f utm;
 #ifdef GPS_USE_LATLONG
   /* Recompute UTM coordinates in this zone */
@@ -149,6 +149,9 @@ void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatV
 }
 
 void ins_realign_v(float z __attribute__ ((unused))) {
+  ins_alt = z;
+  ins_alt_dot = 0.;
+  alt_kalman_reset();
 }
 
 void ins_propagate() {

sw/airborne/subsystems/ins/ins_alt_float.h

@@ -42,14 +42,6 @@ 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,7 +76,7 @@ void ins_periodic( void ) {
     ins.status = INS_RUNNING;
 }
 
-void ins_reset_ground_ref( void ) {
+void ins_reset_local_origin( void ) {
   ins_impl.ltp_initialized = FALSE;
 }
 

sw/airborne/subsystems/ins/ins_float_invariant.c

@@ -273,7 +273,7 @@ void ins_periodic(void) {}
 void ins_propagate(void) {}
 
 
-void ins_reset_ground_ref( void ) {
+void ins_reset_local_origin( void ) {
 #if INS_UPDATE_FW_ESTIMATOR
   struct UtmCoor_f utm;
 #ifdef GPS_USE_LATLONG

sw/airborne/subsystems/ins/ins_gps_passthrough.c

@@ -45,7 +45,7 @@ void ins_init() {
 void ins_periodic( void ) {
 }
 
-void ins_reset_ground_ref( void ) {
+void ins_reset_local_origin( void ) {
   struct UtmCoor_f utm;
 #ifdef GPS_USE_LATLONG
   /* Recompute UTM coordinates in this zone */

sw/airborne/subsystems/ins/ins_int.c

@@ -117,6 +117,7 @@ static void send_ins_ref(void) {
 
 static void ins_init_origin_from_flightplan(void);
 static void ins_ned_to_state(void);
+static void ins_update_from_vff(void);
 #if USE_HFF
 static void ins_update_from_hff(void);
 #endif
@@ -170,7 +171,7 @@ void ins_periodic(void) {
     ins.status = INS_RUNNING;
 }
 
-void ins_reset_ground_ref( void ) {
+void ins_reset_local_origin( void ) {
   ins_impl.ltp_initialized = FALSE;
 #if USE_HFF
   ins.hf_realign = TRUE;
@@ -190,6 +191,7 @@ void ins_reset_altitude_ref( void ) {
 #if USE_HFF
 void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
   b2_hff_realign(pos, speed);
+  ins_update_from_hff();
 }
 #else
 void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)),
@@ -199,6 +201,7 @@ void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)),
 
 void ins_realign_v(float z) {
   vff_realign(z);
+  ins_update_from_vff();
 }
 
 void ins_propagate() {
@@ -211,9 +214,7 @@ void ins_propagate() {
   float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
   if (ins_impl.baro_initialized) {
     vff_propagate(z_accel_meas_float);
-    ins_impl.ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
+    ins_update_from_vff();
-    ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
-    ins_impl.ltp_pos.z   = POS_BFP_OF_REAL(vff_z);
   }
   else { // feed accel from the sensors
     // subtract -9.81m/s2 (acceleration measured due to gravity,
@@ -243,9 +244,7 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres
     ins.vf_realign = FALSE;
     ins_impl.qfe = *pressure;
     vff_realign(0.);
-    ins_impl.ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
+    ins_update_from_vff();
-    ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
-    ins_impl.ltp_pos.z   = POS_BFP_OF_REAL(vff_z);
   }
   else {
     ins_impl.baro_z = -pprz_isa_height_of_pressure(*pressure, ins_impl.qfe);
@@ -411,6 +410,12 @@ static void ins_ned_to_state(void) {
 #endif
 }
 
+/** update ins state from vertical filter */
+static void ins_update_from_vff(void) {
+  ins_impl.ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
+  ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
+  ins_impl.ltp_pos.z   = POS_BFP_OF_REAL(vff_z);
+}
 
 #if USE_HFF
 /** update ins state from horizontal filter */

sw/airborne/subsystems/navigation/common_nav.c

@@ -82,7 +82,7 @@ unit_t nav_reset_utm_zone(void) {
 /** Reset the geographic reference to the current GPS fix */
 unit_t nav_reset_reference( void ) {
   /* realign INS */
-  ins_reset_ground_ref();
+  ins_reset_local_origin();
 
   /* Set nav UTM ref */
   nav_utm_east0 = state.utm_origin_f.east;