From 03d078e6b4021aae3080d05d1d1ec288163d138a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 9 Sep 2014 18:49:26 +0200 Subject: [PATCH] [airborne] send statistics for ahrs/gyro dt in STATE_FILTER_STATUS --- conf/messages.xml | 5 ++- conf/telemetry/default_rotorcraft.xml | 2 + conf/units.xml | 2 + sw/airborne/firmwares/fixedwing/main_ap.c | 27 ++++++++++++- sw/airborne/firmwares/rotorcraft/main.c | 43 +++++++++++++++++++++ sw/airborne/subsystems/ahrs/ahrs_infrared.c | 4 +- 6 files changed, 80 insertions(+), 3 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index c711ad6535a..8dc50b75f94 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1991,7 +1991,10 @@ - + + + + diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index 73806655c86..5d725d218bd 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -20,6 +20,7 @@ + @@ -60,6 +61,7 @@ + diff --git a/conf/units.xml b/conf/units.xml index acfd74dff6a..8c5bc09942f 100644 --- a/conf/units.xml +++ b/conf/units.xml @@ -1,6 +1,8 @@ + + diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index d4baf211841..9e487587ae7 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -141,6 +141,12 @@ static inline void on_gyro_event( void ); static inline void on_accel_event( void ); static inline void on_mag_event( void ); volatile uint8_t ahrs_timeout_counter = 0; +// variables for statistics on dt +static float dt_avg = 0.0; +static float dt_sum = 0.0; +static float dt_min = FLT_MAX; +static float dt_max = FLT_MIN; +static uint32_t dt_cnt = 0; //FIXME not the correct place static void send_filter_status(void) { @@ -148,7 +154,15 @@ static void send_filter_status(void) { if (ahrs.status == AHRS_UNINIT) mde = 2; if (ahrs_timeout_counter > 10) mde = 5; uint16_t val = 0; - DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val); + if (dt_cnt > 0) + dt_avg = dt_sum / dt_cnt; + DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val, + &dt_avg, &dt_min, &dt_max); + // reset dt statistics + dt_sum = 0.0; + dt_cnt = 0; + dt_min = FLT_MAX; + dt_max = FLT_MIN; } #endif // USE_AHRS && USE_IMU @@ -753,10 +767,21 @@ PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.") // dt between this and last callback in seconds float dt = (float)(now_ts - last_ts) / 1e6; last_ts = now_ts; + + // get some statistics on dt + dt_sum += dt; + dt_cnt++; + if (dt < dt_min) + dt_min = dt; + if (dt > dt_max) + dt_max = dt; #else PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS/INS propagation.") PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); + dt_avg = dt; + dt_min = dt; + dt_max = dt; #endif ahrs_timeout_counter = 0; diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index ae94534f49a..c211ebc3848 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -114,6 +114,31 @@ static inline void on_accel_event( void ); static inline void on_gps_event( void ); static inline void on_mag_event( void ); +volatile uint8_t ahrs_timeout_counter = 0; +// variables for statistics on dt +static float dt_avg = 0.0; +static float dt_sum = 0.0; +static float dt_min = FLT_MAX; +static float dt_max = FLT_MIN; +static uint32_t dt_cnt = 0; + +//FIXME not the correct place +static void send_filter_status(void) { + uint8_t mde = 3; + if (ahrs.status == AHRS_UNINIT) mde = 2; + if (ahrs_timeout_counter > 10) mde = 5; + uint16_t val = 0; + if (dt_cnt > 0) + dt_avg = dt_sum / dt_cnt; + DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val, + &dt_avg, &dt_min, &dt_max); + // reset dt statistics + dt_sum = 0.0; + dt_cnt = 0; + dt_min = FLT_MAX; + dt_max = FLT_MIN; +} + tid_t main_periodic_tid; ///< id for main_periodic() timer tid_t modules_tid; ///< id for modules_periodic_task() timer @@ -187,6 +212,8 @@ STATIC_INLINE void main_init( void ) { #if USE_BARO_BOARD baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL); #endif + + register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); } STATIC_INLINE void handle_periodic_tasks( void ) { @@ -272,6 +299,9 @@ STATIC_INLINE void failsafe_check( void ) { #endif autopilot_check_in_flight(autopilot_motors_on); + + if (ahrs_timeout_counter < 255) + ahrs_timeout_counter ++; } STATIC_INLINE void main_event( void ) { @@ -339,14 +369,27 @@ PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.") // dt between this and last callback in seconds float dt = (float)(now_ts - last_ts) / 1e6; last_ts = now_ts; + + // get some statistics on dt + dt_sum += dt; + dt_cnt++; + if (dt < dt_min) + dt_min = dt; + if (dt > dt_max) + dt_max = dt; #else PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS/INS propagation.") PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); + dt_avg = dt; + dt_min = dt; + dt_max = dt; #endif ImuScaleGyro(imu); + ahrs_timeout_counter = 0; + if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.c b/sw/airborne/subsystems/ahrs/ahrs_infrared.c index 648d0494e83..3f71533c401 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_infrared.c +++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.c @@ -51,7 +51,9 @@ static void send_status(void) { uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint8_t mde = 3; if (contrast < 50) mde = 7; - DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &contrast); + const float foo = 0.; + DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &contrast, + &foo, &foo, &foo); } #endif