From 5ad33c5f639118b8e76e8fec14ef523de01ae8b2 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 9 Sep 2014 17:41:22 +0200 Subject: [PATCH] [ins] alt_float, bound dt and used fixed GPS_DT if defined --- sw/airborne/subsystems/ins/ins_alt_float.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index a37ac328a43..80ba59bc4ee 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -144,6 +144,9 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres float dt = now_ts - last_ts; last_ts = now_ts; + // bound dt (assume baro freq 1Hz-500Hz + Bound(dt, 0.002, 1.0) + if (!ins_impl.baro_initialized) { ins_impl.qfe = *pressure; ins_impl.baro_initialized = TRUE; @@ -181,6 +184,9 @@ void ins_update_gps(void) { utm.zone = nav_utm_zone0; #if !USE_BAROMETER +#ifdef GPS_DT + const float dt = GPS_DT; +#else // timestamp when last callback was received static float last_ts = 0.f; // current timestamp @@ -189,6 +195,10 @@ void ins_update_gps(void) { float dt = now_ts - last_ts; last_ts = now_ts; + // bound dt (assume GPS freq between 0.5Hz and 50Hz) + Bound(dt, 0.02, 2) +#endif + float falt = gps.hmsl / 1000.0f; if (ins_impl.reset_alt_ref) { ins_impl.reset_alt_ref = FALSE;