[airborne] calc dt with usec resolution

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -714,14 +714,17 @@ static inline void on_gps_solution( void ) {
 #if USE_IMU
 static inline void on_accel_event( void ) {
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
-  // timestamp when last callback was received
-  static float last_ts = 0.f;
+PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.")
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
   // current timestamp
-  float now_ts = get_sys_time_float();
-  // dt between this and last callback
-  float dt = now_ts - last_ts;
+  uint32_t now_ts = get_sys_time_usec();
+  // dt between this and last callback in seconds
+  float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
 #else
+PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.")
+PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
   const float dt = 1./AHRS_CORRECT_FREQUENCY;
 #endif
 
@@ -734,17 +737,17 @@ static inline void on_accel_event( void ) {
 static inline void on_gyro_event( void ) {
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
 PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
-  // timestamp when last callback was received
-  static float last_ts = 0.f;
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
   // current timestamp
-  float now_ts = get_sys_time_float();
-  // dt between this and last callback
-  float dt = now_ts - last_ts;
+  uint32_t now_ts = get_sys_time_usec();
+  // dt between this and last callback in seconds
+  float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
 #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);
+  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
 #endif
 
   ahrs_timeout_counter = 0;
@@ -777,15 +780,18 @@ static inline void on_mag_event(void)
 {
 #if USE_MAGNETOMETER
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
-  // timestamp when last callback was received
-  static float last_ts = 0.f;
+PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
   // current timestamp
-  float now_ts = get_sys_time_float();
-  // dt between this and last callback
-  float dt = now_ts - last_ts;
+  uint32_t now_ts = get_sys_time_usec();
+  // dt between this and last callback in seconds
+  float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
 #else
-  const float dt = 1./AHRS_MAG_CORRECT_FREQUENCY;
+PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.")
+PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
+  const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
 #endif
 
   ImuScaleMag(imu);

sw/airborne/firmwares/rotorcraft/main.c

@@ -313,17 +313,17 @@ STATIC_INLINE void main_event( void ) {
 static inline void on_accel_event( void ) {
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
 PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.")
-  // timestamp when last callback was received
-  static float last_ts = 0.f;
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
   // current timestamp
-  float now_ts = get_sys_time_float();
+  uint32_t now_ts = get_sys_time_usec();
   // dt between this and last callback
-  float dt = now_ts - last_ts;
+  float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
 #else
 PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.")
 PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
-  const float dt = (1./AHRS_CORRECT_FREQUENCY);
+  const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
 #endif
 
   ImuScaleAccel(imu);
@@ -336,17 +336,17 @@ PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
 static inline void on_gyro_event( void ) {
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
 PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
-  // timestamp when last callback was received
-  static float last_ts = 0.f;
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
   // current timestamp
-  float now_ts = get_sys_time_float();
-  // dt between this and last callback
-  float dt = now_ts - last_ts;
+  uint32_t now_ts = get_sys_time_usec();
+  // dt between this and last callback in seconds
+  float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
 #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);
+  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
 #endif
 
   ImuScaleGyro(imu);
@@ -383,17 +383,17 @@ static inline void on_mag_event(void) {
 #if USE_MAGNETOMETER
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
 PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
-  // timestamp when last callback was received
-  static float last_ts = 0.f;
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
   // current timestamp
-  float now_ts = get_sys_time_float();
-  // dt between this and last callback
-  float dt = now_ts - last_ts;
+  uint32_t now_ts = get_sys_time_usec();
+  // dt between this and last callback in seconds
+  float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
 #else
 PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.")
 PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
-  const float dt = (1./AHRS_MAG_CORRECT_FREQUENCY);
+  const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
 #endif
 
   if (ahrs.status == AHRS_RUNNING) {

sw/airborne/mcu_periph/sys_time.h

@@ -118,7 +118,7 @@ static inline bool_t sys_time_check_and_ack_timer(tid_t id) {
 
 /**
  * Get the time in seconds since startup.
- * @return current system time as float
+ * @return current system time as float with sys_time.resolution
  */
 static inline float get_sys_time_float(void) {
   return (float)(sys_time.nb_sec + (float)(sys_time.nb_sec_rem) / sys_time.cpu_ticks_per_sec);

sw/airborne/subsystems/ins/ins_alt_float.c

@@ -136,12 +136,12 @@ void ins_reset_altitude_ref(void) {
 
 #if USE_BAROMETER
 static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) {
-  // timestamp when last callback was received
-  static float last_ts = 0.f;
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
   // current timestamp
-  float now_ts = get_sys_time_float();
-  // dt between this and last callback
-  float dt = now_ts - last_ts;
+  uint32_t now_ts = get_sys_time_usec();
+  // dt between this and last callback in seconds
+  float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
 
   // bound dt (assume baro freq 1Hz-500Hz
@@ -187,12 +187,12 @@ void ins_update_gps(void) {
 #ifdef GPS_DT
   const float dt = GPS_DT;
 #else
-  // timestamp when last callback was received
-  static float last_ts = 0.f;
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
   // current timestamp
-  float now_ts = get_sys_time_float();
-  // dt between this and last callback
-  float dt = now_ts - last_ts;
+  uint32_t now_ts = get_sys_time_usec();
+  // dt between this and last callback in seconds
+  float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
 
   // bound dt (assume GPS freq between 0.5Hz and 50Hz)

sw/airborne/test/subsystems/test_ahrs.c

@@ -83,12 +83,12 @@ static inline void main_event_task( void ) {
 }
 
 static inline void on_gyro_event(void) {
-  // timestamp when last callback was received
-  static float last_ts = 0.f;
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
   // current timestamp
-  float now_ts = get_sys_time_float();
-  // dt between this and last callback
-  float dt = now_ts - last_ts;
+  uint32_t now_ts = get_sys_time_usec();
+  // dt between this and last callback in seconds
+  float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
 
   ImuScaleGyro(imu);
@@ -105,18 +105,34 @@ static inline void on_gyro_event(void) {
 }
 
 static inline void on_accel_event(void) {
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
+  // dt between this and last callback in seconds
+  float dt = (float)(now_ts - last_ts) / 1e6;
+  last_ts = now_ts;
+
   ImuScaleAccel(imu);
   if (ahrs.status != AHRS_UNINIT) {
     DEBUG_S2_ON();
-    ahrs_update_accel();
+    ahrs_update_accel(dt);
     DEBUG_S2_OFF();
   }
 }
 
 static inline void on_mag_event(void) {
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
+  // dt between this and last callback in seconds
+  float dt = (float)(now_ts - last_ts) / 1e6;
+  last_ts = now_ts;
+
   ImuScaleMag(imu);
   if (ahrs.status == AHRS_RUNNING) {
-    ahrs_update_mag();
+    ahrs_update_mag(dt);
   }
 }