diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 19f63dea9f..53996552d1 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/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) { diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 15cfebbc3a..9a6966c081 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/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; } diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index 75729b341a..9723fa5d7e 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/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); diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index d1f7573218..2e22269be7 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/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