[ins] remove hf|vf_realign from global ins struct

and fix some missing fuctions

conf/settings/control/rotorcraft_guidance.xml

@@ -10,7 +10,6 @@
       <dl_setting var="guidance_v_nominal_throttle" min="0.2" step="0.01" max="0.8" module="guidance/guidance_v" shortname="nominal_throttle" param="GUIDANCE_V_NOMINAL_HOVER_THROTTLE"/>
       <dl_setting var="guidance_v_adapt_throttle_enabled" min="0" step="1" max="1" module="guidance/guidance_v" shortname="adapt_throttle" param="GUIDANCE_V_ADAPT_THROTTLE_ENABLED" values="FALSE|TRUE"/>
       <dl_setting var="guidance_v_z_sp" min="-5" step="0.5" max="3" module="guidance/guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
-      <dl_setting var="ins.vf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="vf_realign" values="OFF|ON"/>
     </dl_settings>
 
     <dl_settings NAME="Horiz Loop">
@@ -22,7 +21,6 @@
       <dl_setting var="guidance_h_again" min="0" step="1" max="400" module="guidance/guidance_h" shortname="ka" param="GUIDANCE_H_AGAIN"/>
       <dl_setting var="guidance_h_pos_sp.x" MIN="-10" MAX="10" STEP="1"  module="guidance/guidance_h" shortname="sp_x_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
       <dl_setting var="guidance_h_pos_sp.y" MIN="-10" MAX="10" STEP="1"  module="guidance/guidance_h" shortname="sp_y_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
-      <dl_setting var="ins.hf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="hf_realign" values="OFF|ON"/>
     </dl_settings>
 
     <dl_settings NAME="NAV">

sw/airborne/modules/ins/ins_xsens.c

@@ -256,6 +256,25 @@ void ins_periodic(void) {
   xsens_periodic();
 }
 
+void ins_reset_local_origin(void) {
+}
+
+void ins_reset_altitude_ref(void) {
+}
+
+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_update_gps(void) {
   struct UtmCoor_f utm;
   utm.east = gps.utm_pos.east / 100.;

sw/airborne/subsystems/imu/imu_um6.c

@@ -187,10 +187,6 @@ void imu_impl_init(void) {
 
 
 void imu_periodic(void) {
-  if (ins.vf_realign == TRUE) {
-    UM6_imu_align();
-  }
-
   /* We would request for data here - optional
   //GET_DATA command 0xAE
   buf_out[0] = 's';

sw/airborne/subsystems/imu/imu_um6.h

@@ -37,7 +37,6 @@
 #include "subsystems/imu.h"
 #include "mcu_periph/uart.h"
 #include "subsystems/ahrs.h"
-#include "subsystems/ins.h"
 
 #define __UM6Link(dev, _x) dev##_x
 #define _UM6Link(dev, _x)  __UM6Link(dev, _x)
@@ -107,35 +106,35 @@ struct UM6Packet {
 };
 
 enum UM6PacketStatus {
-    UM6PacketWaiting,
-    UM6PacketReadingS,
-    UM6PacketReadingN,
-    UM6PacketReadingPT,
-    UM6PacketReadingAddr,
-    UM6PacketReadingData
+  UM6PacketWaiting,
+  UM6PacketReadingS,
+  UM6PacketReadingN,
+  UM6PacketReadingPT,
+  UM6PacketReadingAddr,
+  UM6PacketReadingData
 };
 
 enum UM6Status {
   UM6Uninit,
   UM6Running
 };
 
-#define imu_um6_event(_callback1, _callback2, _callback3) {				\
-    if (UM6Buffer()) {							\
-      ReadUM6Buffer();							\
-    }									\
-    if (UM6_packet.msg_available) {					\
-      UM6_packet.msg_available = FALSE;					\
-      UM6_packet_read_message();					\
-      _callback1();			\
-      _callback2();			\
-      _callback3();     \
-    } \
-}
+#define imu_um6_event(_callback1, _callback2, _callback3) { \
+    if (UM6Buffer()) {                                      \
+      ReadUM6Buffer();                                      \
+    }                                                       \
+    if (UM6_packet.msg_available) {                         \
+      UM6_packet.msg_available = FALSE;                     \
+      UM6_packet_read_message();                            \
+      _callback1();                                         \
+      _callback2();                                         \
+      _callback3();                                         \
+    }                                                       \
+  }
 
-#define ReadUM6Buffer() {						\
-    while (UM6Link(ChAvailable())&&!UM6_packet.msg_available)		\
-      UM6_packet_parse(UM6Link(Getch()));				\
+#define ReadUM6Buffer() {                                       \
+    while (UM6Link(ChAvailable())&&!UM6_packet.msg_available)   \
+      UM6_packet_parse(UM6Link(Getch()));                       \
   }
 
 #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \

sw/airborne/subsystems/ins.h

@@ -32,8 +32,10 @@
 #include "math/pprz_algebra_float.h"
 #include "state.h"
 
-#define INS_UNINIT  0
-#define INS_RUNNING 1
+enum InsStatus {
+ INS_UNINIT=0,
+ INS_RUNNING=1
+};
 
 /* underlying includes (needed for parameters) */
 #ifdef INS_TYPE_H
@@ -42,9 +44,7 @@
 
 /** Inertial Navigation System state */
 struct Ins {
-  uint8_t status;     ///< status of the INS
-  bool_t hf_realign;  ///< flag to request to realign horizontal filter to current position (local origin unchanged)
-  bool_t vf_realign;  ///< flag to request to realign vertical filter to ground level (local origin unchanged)
+  enum InsStatus status;     ///< status of the INS
 };
 
 /** global INS state */
@@ -53,24 +53,24 @@ extern struct Ins ins;
 /** INS initialization. Called at startup.
  *  Needs to be implemented by each INS algorithm.
  */
-extern void ins_init( void );
+extern void ins_init(void);
 
 /** INS periodic call.
  *  Needs to be implemented by each INS algorithm.
  */
-extern void ins_periodic( void );
+extern void ins_periodic(void);
 
 /** 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_local_origin( void );
+extern void ins_reset_local_origin(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 );
+extern void ins_reset_altitude_ref(void);
 
 /** INS utm zone reset.
  *  Reset UTM zone according te the actual position.
@@ -82,42 +82,42 @@ 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.
+ *  Needs to be implemented by each INS algorithm.
  *  @param pos new horizontal position to set
  *  @param speed new horizontal speed to set
- *  Needs to be implemented by each INS algorithm.
  */
 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.
+ *  @param z new altitude to set
  */
 extern void ins_realign_v(float z);
 
 /** Propagation. Usually integrates the gyro rates to angles.
  *  Reads the global #imu data struct.
  *  Needs to be implemented by each INS algorithm.
  */
-extern void ins_propagate( void );
+extern void ins_propagate(void);
 
 /** Update INS state with barometer measurements.
  *  Reads the global #baro data struct.
  *  Needs to be implemented by each INS algorithm.
  */
-extern void ins_update_baro( void );
+extern void ins_update_baro(void);
 
 /** Update INS state with GPS measurements.
  *  Reads the global #gps data struct.
  *  Needs to be implemented by each INS algorithm.
  */
-extern void ins_update_gps( void );
+extern void ins_update_gps(void);
 
 /** Update INS state with sonar measurements.
  *  Reads the global #sonar data struct.
  *  Needs to be implemented by each INS algorithm.
  */
-extern void ins_update_sonar( void );
+extern void ins_update_sonar(void);
 
 
 #endif /* INS_H */