diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
index 30c025f61c..2de29148d4 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
@@ -28,6 +28,8 @@
 #include "subsystems/ahrs.h"
 #include "subsystems/abi.h"
 
+static uint32_t ahrs_fc_last_stamp;
+
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
 #include "mcu_periph/sys_time.h"
@@ -58,7 +60,6 @@ static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
 #ifndef AHRS_FC_FILTER_ID
 #define AHRS_FC_FILTER_ID 5
 #endif
-static uint32_t ahrs_fc_last_stamp;
 
 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
 {
@@ -90,9 +91,9 @@ static abi_event gps_ev;
 
 
 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
-                    uint32_t __attribute__((unused)) stamp,
-                    struct Int32Rates *gyro)
+                    uint32_t stamp, struct Int32Rates *gyro)
 {
+  ahrs_fc_last_stamp = stamp;
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
   PRINT_CONFIG_MSG("Calculating dt for AHRS_FC propagation.")
   /* timestamp in usec when last callback was received */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c
index 39017c099f..3f3ba950bc 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c
@@ -28,6 +28,8 @@
 #include "subsystems/ahrs.h"
 #include "subsystems/abi.h"
 
+static uint32_t ahrs_dcm_last_stamp;
+
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
 #include "mcu_periph/sys_time.h"
@@ -35,7 +37,6 @@
 #ifndef AHRS_DCM_FILTER_ID
 #define AHRS_DCM_FILTER_ID 6
 #endif
-static uint32_t ahrs_dcm_last_stamp;
 
 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
 {
@@ -65,9 +66,9 @@ static abi_event gps_ev;
 
 
 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
-                    uint32_t __attribute__((unused)) stamp,
-                    struct Int32Rates *gyro)
+                    uint32_t stamp, struct Int32Rates *gyro)
 {
+  ahrs_dcm_last_stamp = stamp;
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
   PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.")
   /* timestamp in usec when last callback was received */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
index b2feee3fb2..34eeb24301 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
@@ -29,6 +29,8 @@
 #include "subsystems/ahrs.h"
 #include "subsystems/abi.h"
 
+static uint32_t ahrs_mlkf_last_stamp;
+
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
 #include "mcu_periph/sys_time.h"
@@ -42,7 +44,6 @@ static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
 #ifndef AHRS_MLKF_FILTER_ID
 #define AHRS_MLKF_FILTER_ID 6
 #endif
-static uint32_t ahrs_mlkf_last_stamp;
 
 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
 {
@@ -73,9 +74,9 @@ static abi_event geo_mag_ev;
 
 
 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
-                    uint32_t __attribute__((unused)) stamp,
-                    struct Int32Rates *gyro)
+                    uint32_t stamp, struct Int32Rates *gyro)
 {
+  ahrs_mlkf_last_stamp = stamp;
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
   PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.")
   /* timestamp in usec when last callback was received */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
index 402699a825..725e961bc0 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
@@ -28,6 +28,8 @@
 #include "subsystems/ahrs.h"
 #include "subsystems/abi.h"
 
+static uint32_t ahrs_ice_last_stamp;
+
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
 #include "mcu_periph/sys_time.h"
@@ -74,7 +76,6 @@ static void send_bias(struct transport_tx *trans, struct link_device *dev)
 #ifndef AHRS_ICE_FILTER_ID
 #define AHRS_ICE_FILTER_ID 4
 #endif
-static uint32_t ahrs_ice_last_stamp;
 
 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
 {
@@ -103,13 +104,12 @@ static abi_event body_to_imu_ev;
 
 
 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
-                    uint32_t stamp __attribute__((unused)),
-                    struct Int32Rates *gyro)
+                    uint32_t stamp, struct Int32Rates *gyro)
 {
+  ahrs_ice_last_stamp = stamp;
   if (ahrs_ice.is_aligned) {
     ahrs_ice_propagate(gyro);
   }
-
 }
 
 static void accel_cb(uint8_t sender_id __attribute__((unused)),
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
index 01f9de9f08..2c24f60368 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
@@ -28,6 +28,8 @@
 #include "subsystems/ahrs.h"
 #include "subsystems/abi.h"
 
+static uint32_t ahrs_icq_last_stamp;
+
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
 #include "mcu_periph/sys_time.h"
@@ -81,7 +83,6 @@ static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
 #ifndef AHRS_ICQ_FILTER_ID
 #define AHRS_ICQ_FILTER_ID 3
 #endif
-static uint32_t ahrs_icq_last_stamp;
 
 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
 {
@@ -113,9 +114,9 @@ static abi_event gps_ev;
 
 
 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
-                    uint32_t __attribute__((unused)) stamp,
-                    struct Int32Rates *gyro)
+                    uint32_t stamp, struct Int32Rates *gyro)
 {
+  ahrs_icq_last_stamp = stamp;
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
   PRINT_CONFIG_MSG("Calculating dt for AHRS_ICQ propagation.")
   /* timestamp in usec when last callback was received */