[airborne] send statistics for ahrs/gyro dt in STATE_FILTER_STATUS

conf/messages.xml

@@ -1991,7 +1991,10 @@
 
    <message name="STATE_FILTER_STATUS" id="232">
      <field name="state_filter_mode" type="uint8" values="UNKNOWN|INIT|ALIGN|OK|GPS_LOST|IMU_LOST|COV_ERR|IR_CONTRAST|ERROR"/>
-     <field name="value" type="uint16" />
+     <field name="value" type="uint16"/>
+     <field name="dt_avg" type="float" unit="sec" alt_unit="msec"/>
+     <field name="dt_min" type="float" unit="sec" alt_unit="msec"/>
+     <field name="dt_max" type="float" unit="sec" alt_unit="msec"/>
    </message>
 
  <!--233 is free -->

conf/telemetry/default_rotorcraft.xml

@@ -20,6 +20,7 @@
       <message name="UART_ERRORS"            period="3.1"/>
       <message name="SUPERBITRF"             period="3"/>
       <message name="ENERGY"                 period="2.5"/>
+      <message name="STATE_FILTER_STATUS"    period="5."/>
     </mode>
 
     <mode name="ppm">
@@ -60,6 +61,7 @@
 <!--      <message name="AHRS_QUAT_INT"   period=".25"/> -->
       <message name="AHRS_EULER_INT"     period=".1"/>
 <!--      <message name="AHRS_RMAT_INT"   period=".5"/> -->
+      <message name="STATE_FILTER_STATUS" period="1."/>
     </mode>
 
     <mode name="rate_loop">

conf/units.xml

@@ -1,6 +1,8 @@
 <!-- Table of default units convertion -->
 <!-- used to convert from unit to alt_unit (messages) or code_unit (airframe) -->
 <units>
+  <unit from="sec" to="msec" coef="1000."/>
+  <unit from="msec" to="sec" coef="0.001"/>
   <unit from="m" to="cm" coef="100."/>
   <unit from="cm" to="m" coef="0.01"/>
   <unit from="m/s" to="cm/s" coef="100."/>

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -141,14 +141,28 @@ 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) {
   uint8_t mde = 3;
   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;

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)

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