diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h index 771cfd4a91..fa42c8f3ca 100644 --- a/sw/airborne/subsystems/ins.h +++ b/sw/airborne/subsystems/ins.h @@ -32,11 +32,6 @@ #include "math/pprz_algebra_float.h" #include "state.h" -enum InsStatus { - INS_UNINIT = 0, - INS_RUNNING = 1 -}; - /* underlying includes (needed for parameters) */ #ifdef INS_TYPE_H #include INS_TYPE_H @@ -44,7 +39,6 @@ enum InsStatus { /** Inertial Navigation System state */ struct Ins { - enum InsStatus status; ///< status of the INS }; /** global INS state */ diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 848cfcb389..d24bf40a56 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -95,8 +95,6 @@ void ins_init(void) // why do we have this here? alt_kalman(0.0f, 0.1); - - ins.status = INS_RUNNING; } diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c index b081d7f7ca..9d27e334df 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.c +++ b/sw/airborne/subsystems/ins/ins_ardrone2.c @@ -69,13 +69,6 @@ void ins_init() INT32_VECT3_ZERO(ins_impl.ltp_accel); } -void ins_periodic(void) -{ - if (ins_impl.ltp_initialized) { - ins.status = INS_RUNNING; - } -} - void ins_reset_local_origin(void) { #if USE_GPS diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index d5d4e6ed50..d14126bfd1 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -282,7 +282,6 @@ void ins_init() ins_impl.gains.rh = INS_INV_RH; ins_impl.gains.sh = INS_INV_SH; - ins.status = INS_UNINIT; ins_impl.is_aligned = FALSE; ins_impl.reset = FALSE; @@ -357,7 +356,6 @@ void ins_float_invariant_align(struct Int32Rates *lp_gyro, // ins and ahrs are now running ins_impl.is_aligned = TRUE; - ins.status = INS_RUNNING; } void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* accel, float dt) @@ -368,7 +366,6 @@ void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* a // a complete init cycle is required if (ins_impl.reset) { ins_impl.reset = FALSE; - ins.status = INS_UNINIT; ins_impl.is_aligned = FALSE; init_invariant_state(); } @@ -488,7 +485,7 @@ void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* a void ins_update_gps(void) { - if (gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING) { + if (gps.fix == GPS_FIX_3D && ins_impl.is_aligned) { ins_gps_fix_once = TRUE; #if INS_UPDATE_FW_ESTIMATOR @@ -672,7 +669,7 @@ static inline void error_output(struct InsFloatInv *_ins) // pos and speed error only if GPS data are valid // or while waiting first GPS data to prevent diverging - if ((gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING + if ((gps.fix == GPS_FIX_3D && ins_impl.is_aligned #if INS_UPDATE_FW_ESTIMATOR && state.utm_initialized_f #else diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index 7de8c6dcc2..3ad4e5c8f2 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -119,14 +119,6 @@ void ins_init(void) #endif } -void ins_periodic(void) -{ - if (ins_impl.ltp_initialized) { - ins.status = INS_RUNNING; - } -} - - void ins_reset_local_origin(void) { ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos); diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c index f655f47f99..57f6830bb4 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c @@ -40,8 +40,6 @@ void ins_init(void) struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); - - ins.status = INS_RUNNING; } void ins_reset_local_origin(void) diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index ee73131fba..c3a5531717 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -194,13 +194,6 @@ void ins_init(void) #endif } -void ins_periodic(void) -{ - if (ins_impl.ltp_initialized) { - ins.status = INS_RUNNING; - } -} - void ins_reset_local_origin(void) { #if USE_GPS