[ins] fix ins_reset_local_origin ifndef GPS_USE_LATLONG

divide utm in int/cm by float (100.0f instead of 100)

while at it, explicitly use floats in some more conversions (technically 100. is a double)

sw/airborne/subsystems/ins/ins_alt_float.c

@@ -78,15 +78,15 @@ void ins_init(void) {
   alt_kalman_init();
 
 #if USE_BAROMETER
-  ins_impl.qfe = 0;;
+  ins_impl.qfe = 0.0f;
   ins_impl.baro_initialized = FALSE;
-  ins_impl.baro_alt = 0.;
+  ins_impl.baro_alt = 0.0f;
   // Bind to BARO_ABS message
   AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
 #endif
   ins_impl.reset_alt_ref = FALSE;
 
-  alt_kalman(0.);
+  alt_kalman(0.0f);
 
   ins.status = INS_RUNNING;
 }
@@ -98,17 +98,17 @@ void ins_reset_local_origin(void) {
 #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;
+  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;
+  utm.east = gps.utm_pos.east / 100.0f;
+  utm.north = gps.utm_pos.north / 100.0f;
 #endif
   // ground_alt
-  utm.alt = gps.hmsl/1000.;
+  utm.alt = gps.hmsl  /1000.0f;
 
   // reset state UTM ref
   stateSetLocalUtmOrigin_f(&utm);
@@ -120,7 +120,7 @@ void ins_reset_local_origin(void) {
 void ins_reset_altitude_ref(void) {
   struct UtmCoor_f utm = state.utm_origin_f;
   // ground_alt
-  utm.alt = gps.hmsl/1000.;
+  utm.alt = gps.hmsl / 1000.0f;
   // reset state UTM ref
   stateSetLocalUtmOrigin_f(&utm);
   // reset filter flag
@@ -137,7 +137,7 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres
   if (ins_impl.reset_alt_ref) {
     ins_impl.reset_alt_ref = FALSE;
     ins_impl.alt = ground_alt;
-    ins_impl.alt_dot = 0.;
+    ins_impl.alt_dot = 0.0f;
     ins_impl.qfe = *pressure;
     alt_kalman_reset();
   }
@@ -162,22 +162,22 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres
 void ins_update_gps(void) {
 #if USE_GPS
   struct UtmCoor_f utm;
-  utm.east = gps.utm_pos.east / 100.;
-  utm.north = gps.utm_pos.north / 100.;
+  utm.east = gps.utm_pos.east / 100.0f;
+  utm.north = gps.utm_pos.north / 100.0f;
   utm.zone = nav_utm_zone0;
 
 #if !USE_BAROMETER
-  float falt = gps.hmsl / 1000.;
+  float falt = gps.hmsl / 1000.0f;
   alt_kalman(falt);
-  ins_impl.alt_dot = -gps.ned_vel.z / 100.;
+  ins_impl.alt_dot = -gps.ned_vel.z / 100.0f;
 #endif
   utm.alt = ins_impl.alt;
   // set position
   stateSetPositionUtm_f(&utm);
 
   struct NedCoor_f ned_vel = {
-    gps.ned_vel.x / 100.,
-    gps.ned_vel.y / 100.,
+    gps.ned_vel.x / 100.0f,
+    gps.ned_vel.y / 100.0f,
     -ins_impl.alt_dot
   };
   // set velocity
@@ -196,10 +196,10 @@ void ins_update_gps(void) {
 static float p[2][2];
 
 static void alt_kalman_reset(void) {
-  p[0][0] = 1.;
-  p[0][1] = 0.;
-  p[1][0] = 0.;
-  p[1][1] = 1.;
+  p[0][0] = 1.0f;
+  p[0][1] = 0.0f;
+  p[1][0] = 0.0f;
+  p[1][1] = 1.0f;
 }
 
 static void alt_kalman_init(void) {

sw/airborne/subsystems/ins/ins_float_invariant.c

@@ -202,13 +202,13 @@ static inline void init_invariant_state(void) {
   FLOAT_RATES_ZERO(ins_impl.state.bias);
   FLOAT_VECT3_ZERO(ins_impl.state.pos);
   FLOAT_VECT3_ZERO(ins_impl.state.speed);
-  ins_impl.state.as = 1.;
-  ins_impl.state.hb = 0.;
+  ins_impl.state.as = 1.0f;
+  ins_impl.state.hb = 0.0f;
 
   // init measures
   FLOAT_VECT3_ZERO(ins_impl.meas.pos_gps);
   FLOAT_VECT3_ZERO(ins_impl.meas.speed_gps);
-  ins_impl.meas.baro_alt = 0.;
+  ins_impl.meas.baro_alt = 0.0f;
 
   // init baro
   ins_baro_initialized = FALSE;
@@ -275,17 +275,17 @@ void ins_reset_local_origin( void ) {
 #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;
+  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;
+  utm.east = gps.utm_pos.east / 100.0f;
+  utm.north = gps.utm_pos.north / 100.0f;
 #endif
   // ground_alt
-  utm.alt = gps.hmsl/1000.;
+  utm.alt = gps.hmsl / 1000.0f;
   // reset state UTM ref
   stateSetLocalUtmOrigin_f(&utm);
 #else
@@ -299,7 +299,7 @@ void ins_reset_local_origin( void ) {
 void ins_reset_altitude_ref( void ) {
 #if INS_UPDATE_FW_ESTIMATOR
   struct UtmCoor_f utm = state.utm_origin_f;
-  utm.alt = gps.hmsl/1000.;
+  utm.alt = gps.hmsl / 1000.0f;
   stateSetLocalUtmOrigin_f(&utm);
 #else
   struct LtpDef_i ltp_def = state.ned_origin_i;
@@ -449,13 +449,13 @@ void ahrs_update_gps(void) {
 #if INS_UPDATE_FW_ESTIMATOR
     if (state.utm_initialized_f) {
       // position (local ned)
-      ins_impl.meas.pos_gps.x = (gps.utm_pos.north / 100.) - state.utm_origin_f.north;
-      ins_impl.meas.pos_gps.y = (gps.utm_pos.east / 100.) - state.utm_origin_f.east;
-      ins_impl.meas.pos_gps.z = state.utm_origin_f.alt - (gps.hmsl / 1000.);
+      ins_impl.meas.pos_gps.x = (gps.utm_pos.north / 100.0f) - state.utm_origin_f.north;
+      ins_impl.meas.pos_gps.y = (gps.utm_pos.east / 100.0f) - state.utm_origin_f.east;
+      ins_impl.meas.pos_gps.z = state.utm_origin_f.alt - (gps.hmsl / 1000.0f);
       // speed
-      ins_impl.meas.speed_gps.x = gps.ned_vel.x / 100.;
-      ins_impl.meas.speed_gps.y = gps.ned_vel.y / 100.;
-      ins_impl.meas.speed_gps.z = gps.ned_vel.z / 100.;
+      ins_impl.meas.speed_gps.x = gps.ned_vel.x / 100.0f;
+      ins_impl.meas.speed_gps.y = gps.ned_vel.y / 100.0f;
+      ins_impl.meas.speed_gps.z = gps.ned_vel.z / 100.0f;
     }
 #else
     if (state.ned_initialized_f) {
@@ -463,11 +463,11 @@ void ahrs_update_gps(void) {
       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.);
+      VECT3_SDIV(ins_impl.meas.pos_gps, gps_pos_cm_ned, 100.0f);
       struct NedCoor_f gps_speed_cm_s_ned;
       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.);
+      VECT3_SDIV(ins_impl.meas.speed_gps, gps_speed_cm_s_ned, 100.0f);
     }
 #endif
   }
@@ -476,11 +476,11 @@ void ahrs_update_gps(void) {
 
 
 static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) {
-  static float ins_qfe = 101325.0;
-  static float alpha = 10.;
+  static float ins_qfe = 101325.0f;
+  static float alpha = 10.0f;
   static int32_t i = 1;
-  static float baro_moy = 0.;
-  static float baro_prev = 0.;
+  static float baro_moy = 0.0f;
+  static float baro_prev = 0.0f;
 
   if (!ins_baro_initialized) {
     // try to find a stable qfe
@@ -489,11 +489,11 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres
       baro_moy = *pressure;
       baro_prev = *pressure;
     }
-    baro_moy = (baro_moy*(i-1) + *pressure)/i;
-    alpha = (10.*alpha + (baro_moy-baro_prev))/(11.);
+    baro_moy = (baro_moy*(i-1) + *pressure) / i;
+    alpha = (10.*alpha + (baro_moy-baro_prev)) / (11.0f);
     baro_prev = baro_moy;
     // test stop condition
-    if (fabs(alpha) < 0.005) {
+    if (fabs(alpha) < 0.005f) {
       ins_qfe = baro_moy;
       ins_baro_initialized = TRUE;
     }

sw/airborne/subsystems/ins/ins_gps_passthrough.c

@@ -53,35 +53,35 @@ void ins_reset_local_origin(void) {
   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;
+  utm.east = gps.utm_pos.east / 100.0f;
+  utm.north = gps.utm_pos.north / 100.0f;
 #endif
   // ground_alt
-  utm.alt = gps.hmsl/1000.;
+  utm.alt = gps.hmsl / 1000.0f;
   // 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.;
+  utm.alt = gps.hmsl / 1000.0f;
   stateSetLocalUtmOrigin_f(&utm);
 }
 
 void ins_update_gps(void) {
   struct UtmCoor_f utm;
-  utm.east = gps.utm_pos.east / 100.;
-  utm.north = gps.utm_pos.north / 100.;
+  utm.east = gps.utm_pos.east / 100.0f;
+  utm.north = gps.utm_pos.north / 100.0f;
   utm.zone = nav_utm_zone0;
-  utm.alt = gps.hmsl / 1000.;
+  utm.alt = gps.hmsl / 1000.0f;
 
   // set position
   stateSetPositionUtm_f(&utm);
 
   struct NedCoor_f ned_vel = {
-    gps.ned_vel.x / 100.,
-    gps.ned_vel.y / 100.,
-    gps.ned_vel.z / 100.
+    gps.ned_vel.x / 100.0f,
+    gps.ned_vel.y / 100.0f,
+    gps.ned_vel.z / 100.0f
   };
   // set velocity
   stateSetSpeedNed_f(&ned_vel);

sw/airborne/subsystems/ins/ins_int.c

@@ -264,7 +264,7 @@ void ins_update_gps(void) {
     /* horizontal gps transformed to NED in meters as float */
     struct FloatVect2 gps_pos_m_ned;
     VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y);
-    VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.);
+    VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f);
 
     struct FloatVect2 gps_speed_m_s_ned;
     VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
@@ -272,7 +272,7 @@ void ins_update_gps(void) {
 
     if (ins_impl.hf_realign) {
       ins_impl.hf_realign = FALSE;
-      const struct FloatVect2 zero = {0.0, 0.0};
+      const struct FloatVect2 zero = {0.0f, 0.0f};
       b2_hff_realign(gps_pos_m_ned, zero);
     }
     // run horizontal filter