[autopilot] group tasks and improve timing

- some tasks are grouped in a gnc group as they should be called
in sequence: estimation, control, default, actuators
- the timing between the sensors and gnc is half of the main period, it
correspond to the default resolution of the sys_time virtual timers
- the timing of AP threads is improved with a calculated wakeup time
instead of a fixed sleep time
- the monitor task for fixedwing is moved in the firmware specific
autopilot part and called by a module
- all timings have been accurately tested with high speed log and a
dedicated profiler for ChibiOS

conf/modules/autopilot_gnc_fw.xml

@@ -11,6 +11,7 @@
   </dep>
   <periodic fun="navigation_task()" freq="NAVIGATION_FREQUENCY" cond="FIXEDWING_FIRMWARE @AND !USE_GENERATED_AUTOPILOT"/>
   <periodic fun="attitude_loop()" freq="CONTROL_FREQUENCY" cond="FIXEDWING_FIRMWARE @AND !USE_GENERATED_AUTOPILOT @AND !AHRS_TRIGGERED_ATTITUDE_LOOP"/>
+  <periodic fun="monitor_task()" freq="1"/>
   <makefile target="!fbw"/>
 </module>
 

sw/airborne/firmwares/fixedwing/autopilot_firmware.c

@@ -134,3 +134,58 @@ void autopilot_firmware_init(void)
 #endif
 }
 
+
+#include "generated/flight_plan.h"
+#include "generated/airframe.h"
+
+#ifdef LOW_BATTERY_KILL_DELAY
+#warning LOW_BATTERY_KILL_DELAY has been renamed to CATASTROPHIC_BAT_KILL_DELAY, please update your airframe file!
+#endif
+
+/** Maximum time allowed for catastrophic battery level before going into kill mode */
+#ifndef CATASTROPHIC_BAT_KILL_DELAY
+#define CATASTROPHIC_BAT_KILL_DELAY 5
+#endif
+
+/** Maximum distance from HOME waypoint before going into kill mode */
+#ifndef KILL_MODE_DISTANCE
+#define KILL_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
+#endif
+
+/** Default minimal speed for takeoff in m/s */
+#ifndef MIN_SPEED_FOR_TAKEOFF
+#define MIN_SPEED_FOR_TAKEOFF 5.
+#endif
+
+/** monitor stuff run at 1Hz */
+void monitor_task(void)
+{
+  if (autopilot.flight_time) {
+    autopilot.flight_time++;
+  }
+
+  static uint8_t t = 0;
+  if (ap_electrical.vsupply < CATASTROPHIC_BAT_LEVEL) {
+    t++;
+  } else {
+    t = 0;
+  }
+#if !USE_GENERATED_AUTOPILOT
+  // only check for static autopilot
+  autopilot.kill_throttle |= (t >= CATASTROPHIC_BAT_KILL_DELAY);
+  autopilot.kill_throttle |= autopilot.launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
+#endif
+
+  if (!autopilot.flight_time &&
+      stateGetHorizontalSpeedNorm_f() > MIN_SPEED_FOR_TAKEOFF) {
+    autopilot.flight_time = 1;
+    autopilot.launch = true; /* Not set in non auto launch */
+#if DOWNLINK
+    uint16_t time_sec = sys_time.nb_sec;
+    DOWNLINK_SEND_TAKEOFF(DefaultChannel, DefaultDevice, &time_sec);
+#endif
+  }
+
+}
+
+

sw/airborne/firmwares/fixedwing/autopilot_firmware.h

@@ -51,6 +51,11 @@ extern uint8_t  mcu1_status;
  */
 extern void autopilot_firmware_init(void);
 
+/** monitoring task
+ *  should be called at 1Hz
+ */
+extern void monitor_task(void);
+
 #endif /* AUTOPILOT_FIRMWARE_H */
 
 

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2003-2010  The Paparazzi Team
+ * Copyright (C) 2003-2021  The Paparazzi Team
  *
  * This file is part of paparazzi.
  *
@@ -40,7 +40,6 @@
 #include "inter_mcu.h"
 #include "link_mcu.h"
 
-#include "generated/flight_plan.h"
 #include "generated/airframe.h"
 #include "generated/modules.h"
 #include "subsystems/abi.h"
@@ -94,28 +93,14 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
  */
 tid_t modules_mcu_core_tid; // single step
 tid_t modules_sensors_tid;
-tid_t modules_estimation_tid;
 //tid_t modules_radio_control_tid; // done in FBW
-tid_t modules_control_actuators_tid; // single step
+tid_t modules_gnc_tid; // estimation, control, actuators, default in a single step
 tid_t modules_datalink_tid;
-tid_t modules_default_tid;
-tid_t monitor_tid;     ///< id for monitor_task() timer FIXME
 
 #define SYS_PERIOD (1.f / PERIODIC_FREQUENCY)
+#define SENSORS_PERIOD (1.f / PERIODIC_FREQUENCY)
 #define DATALINK_PERIOD (1.f / TELEMETRY_FREQUENCY)
 
-#ifndef ESTIMATION_OFFSET
-#define ESTIMATION_OFFSET 6e-4f
-#endif
-
-#ifndef CONTROL_OFFSET
-#define CONTROL_OFFSET 7e-4f
-#endif
-
-#ifndef DEFAULT_OFFSET
-#define DEFAULT_OFFSET 8e-4f
-#endif
-
 void init_ap(void)
 {
 #ifndef SINGLE_MCU
@@ -140,16 +125,25 @@ void init_ap(void)
 #endif
 
   // register timers with temporal dependencies
-  modules_sensors_tid = sys_time_register_timer(SYS_PERIOD, NULL);
-  modules_estimation_tid = sys_time_register_timer_offset(modules_sensors_tid, ESTIMATION_OFFSET, NULL);
-  modules_control_actuators_tid = sys_time_register_timer_offset(modules_sensors_tid, CONTROL_OFFSET, NULL);
-  modules_default_tid = sys_time_register_timer_offset(modules_sensors_tid, DEFAULT_OFFSET, NULL); // should it be an offset ?
+  modules_sensors_tid = sys_time_register_timer(SENSORS_PERIOD, NULL);
+
+  // common GNC group (estimation, control, actuators, default)
+  // is called with an offset of half the main period (1/PERIODIC_FREQUENCY)
+  // which is the default resolution of SYS_TIME_FREQUENCY,
+  // hence the resolution of the virtual timers.
+  // In practice, this is the best compromised between having enough time between
+  // the sensor readings (triggerd in sensors task group) and the lag between
+  // the state update and control/actuators update
+  //
+  //      |      PERIODIC_FREQ       |
+  //      |            |             |
+  //      read         gnc
+  //
+  modules_gnc_tid = sys_time_register_timer_offset(modules_sensors_tid, 1.f / (2.f * PERIODIC_FREQUENCY), NULL);
 
   // register the timers for the periodic functions
   modules_mcu_core_tid = sys_time_register_timer(SYS_PERIOD, NULL);
-  //modules_radio_control_tid = sys_time_register_timer((1. / 60.), NULL); // FIXME
   modules_datalink_tid = sys_time_register_timer(DATALINK_PERIOD, NULL);
-  monitor_tid = sys_time_register_timer(1., NULL); // FIXME
 
   /* set initial trim values.
    * these are passed to fbw via inter_mcu.
@@ -164,6 +158,7 @@ void init_ap(void)
   // send body_to_imu from here for now
   AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
 #endif
+
 }
 
 
@@ -173,21 +168,9 @@ void handle_periodic_tasks_ap(void)
     modules_sensors_periodic_task();
   }
 
-  if (sys_time_check_and_ack_timer(modules_estimation_tid)) {
+  if (sys_time_check_and_ack_timer(modules_gnc_tid)) {
     modules_estimation_periodic_task();
-  }
-
-  // done in FBW
-  //if (sys_time_check_and_ack_timer(modules_radio_control_tid)) {
-  //  radio_control_periodic_task();
-  //  modules_radio_control_periodic_task(); // FIXME integrate above
-  //}
-
-  if (sys_time_check_and_ack_timer(modules_control_actuators_tid)) {
     modules_control_periodic_task();
-  }
-
-  if (sys_time_check_and_ack_timer(modules_default_tid)) {
     modules_default_periodic_task();
   }
 
@@ -205,9 +188,6 @@ void handle_periodic_tasks_ap(void)
 #endif
   }
 
-  if (sys_time_check_and_ack_timer(monitor_tid)) {
-    monitor_task();
-  }
 }
 
 
@@ -237,57 +217,6 @@ void reporting_task(void)
 }
 
 
-#ifdef LOW_BATTERY_KILL_DELAY
-#warning LOW_BATTERY_KILL_DELAY has been renamed to CATASTROPHIC_BAT_KILL_DELAY, please update your airframe file!
-#endif
-
-/** Maximum time allowed for catastrophic battery level before going into kill mode */
-#ifndef CATASTROPHIC_BAT_KILL_DELAY
-#define CATASTROPHIC_BAT_KILL_DELAY 5
-#endif
-
-/** Maximum distance from HOME waypoint before going into kill mode */
-#ifndef KILL_MODE_DISTANCE
-#define KILL_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
-#endif
-
-/** Default minimal speed for takeoff in m/s */
-#ifndef MIN_SPEED_FOR_TAKEOFF
-#define MIN_SPEED_FOR_TAKEOFF 5.
-#endif
-
-/** monitor stuff run at 1Hz */
-void monitor_task(void)
-{
-  if (autopilot.flight_time) {
-    autopilot.flight_time++;
-  }
-
-  static uint8_t t = 0;
-  if (ap_electrical.vsupply < CATASTROPHIC_BAT_LEVEL) {
-    t++;
-  } else {
-    t = 0;
-  }
-#if !USE_GENERATED_AUTOPILOT
-  // only check for static autopilot
-  autopilot.kill_throttle |= (t >= CATASTROPHIC_BAT_KILL_DELAY);
-  autopilot.kill_throttle |= autopilot.launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
-#endif
-
-  if (!autopilot.flight_time &&
-      stateGetHorizontalSpeedNorm_f() > MIN_SPEED_FOR_TAKEOFF) {
-    autopilot.flight_time = 1;
-    autopilot.launch = true; /* Not set in non auto launch */
-#if DOWNLINK
-    uint16_t time_sec = sys_time.nb_sec;
-    DOWNLINK_SEND_TAKEOFF(DefaultChannel, DefaultDevice, &time_sec);
-#endif
-  }
-
-}
-
-
 /*********** EVENT ***********************************************************/
 void event_task_ap(void)
 {

sw/airborne/firmwares/fixedwing/main_ap.h

@@ -33,7 +33,6 @@ extern void init_ap(void);
 extern void handle_periodic_tasks_ap(void);
 extern void event_task_ap(void);
 
-extern void monitor_task(void);
 extern void reporting_task(void);
 
 #endif

sw/airborne/firmwares/fixedwing/main_chibios.c

@@ -29,9 +29,7 @@
 
 #ifndef  SYS_TIME_FREQUENCY
 #error SYS_TIME_FREQUENCY should be defined in Makefile.chibios or airframe.xml and be equal to CH_CFG_ST_FREQUENCY
-#elif SYS_TIME_FREQUENCY != CH_CFG_ST_FREQUENCY
-#error SYS_TIME_FREQUENCY should be equal to CH_CFG_ST_FREQUENCY
-#elif  CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY)
+#elif  CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY) && SYS_TIME_FREQUENCY < (2 * PERIODIC_FREQUENCY)
 #error CH_CFG_ST_FREQUENCY and SYS_TIME_FREQUENCY should be >= 2 x PERIODIC_FREQUENCY
 #endif
 
@@ -130,16 +128,14 @@ static void thd_ap(void *arg)
   chRegSetThreadName("AP");
 
   while (!chThdShouldTerminateX()) {
+    systime_t t = chVTGetSystemTime();
     Ap(handle_periodic_tasks);
     Ap(event_task);
-    // In tick mode, the minimum step is 1e6 / CH_CFG_ST_FREQUENCY
-    // which means that whatever happens, if we do a sleep of this
-    // time step, the next wakeup will be "aligned" and we won't see
-    // jitter. The polling on event will also be as fast as possible
-    // Be careful that in tick-less mode, it will be required to use
-    // the chThdSleepUntil function with a correct computation of the
-    // wakeup time, in particular roll-over should be check.
-    chThdSleepMicroseconds(1000000 / CH_CFG_ST_FREQUENCY);
+    // The sleep time is computed to have a polling interval of
+    // 1e6 / CH_CFG_ST_FREQUENCY. If time is passed, thanks to the
+    // "Windowed" sleep function, the execution is not blocked until
+    // a complet roll-over.
+    chThdSleepUntilWindowed(t, t + TIME_US2I(1000000 / CH_CFG_ST_FREQUENCY));
   }
 
   chThdExit(0);
@@ -156,16 +152,14 @@ static void thd_fbw(void *arg)
   chRegSetThreadName("FBW");
 
   while (!chThdShouldTerminateX()) {
+    systime_t t = chVTGetSystemTime();
     Fbw(handle_periodic_tasks);
     Fbw(event_task);
-    // In tick mode, the minimum step is 1e6 / CH_CFG_ST_FREQUENCY
-    // which means that whatever happens, if we do a sleep of this
-    // time step, the next wakeup will be "aligned" and we won't see
-    // jitter. The polling on event will also be as fast as possible
-    // Be careful that in tick-less mode, it will be required to use
-    // the chThdSleepUntil function with a correct computation of the
-    // wakeup time, in particular roll-over should be check.
-    chThdSleepMicroseconds(1000000 / CH_CFG_ST_FREQUENCY);
+    // The sleep time is computed to have a polling interval of
+    // 1e6 / CH_CFG_ST_FREQUENCY. If time is passed, thanks to the
+    // "Windowed" sleep function, the execution is not blocked until
+    // a complet roll-over.
+    chThdSleepUntilWindowed(t, t + TIME_US2I(1000000 / CH_CFG_ST_FREQUENCY));
   }
 
   chThdExit(0);

sw/airborne/firmwares/rotorcraft/main_ap.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2008-2010 The Paparazzi Team
+ * Copyright (C) 2008-2021 The Paparazzi Team
  *
  * This file is part of Paparazzi.
  *
@@ -56,11 +56,6 @@ PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
  */
 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
 
-/* MODULES_FREQUENCY is defined in generated/modules.h
- * according to main_freq parameter set for modules in airframe file
- */
-PRINT_CONFIG_VAR(MODULES_FREQUENCY)
-
 #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
@@ -73,28 +68,15 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
  */
 tid_t modules_mcu_core_tid; // single step
 tid_t modules_sensors_tid;
-tid_t modules_estimation_tid;
 tid_t modules_radio_control_tid;
-tid_t modules_control_actuators_tid; // single step
+tid_t modules_gnc_tid; // estimation, control, actuators, default in a single step
 tid_t modules_datalink_tid;
-tid_t modules_default_tid;
 tid_t failsafe_tid;      ///< id for failsafe_check() timer FIXME
 
 #define SYS_PERIOD (1.f / PERIODIC_FREQUENCY)
+#define SENSORS_PERIOD (1.f / PERIODIC_FREQUENCY)
 #define DATALINK_PERIOD (1.f / TELEMETRY_FREQUENCY)
 
-#ifndef ESTIMATION_OFFSET
-#define ESTIMATION_OFFSET 6e-4f
-#endif
-
-#ifndef CONTROL_OFFSET
-#define CONTROL_OFFSET 7e-4f
-#endif
-
-#ifndef DEFAULT_OFFSET
-#define DEFAULT_OFFSET 8e-4f
-#endif
-
 void main_init(void)
 {
   modules_mcu_init();
@@ -119,10 +101,21 @@ void main_init(void)
 #endif
 
   // register timers with temporal dependencies
-  modules_sensors_tid = sys_time_register_timer(SYS_PERIOD, NULL);
-  modules_estimation_tid = sys_time_register_timer_offset(modules_sensors_tid, ESTIMATION_OFFSET, NULL);
-  modules_control_actuators_tid = sys_time_register_timer_offset(modules_sensors_tid, CONTROL_OFFSET, NULL);
-  modules_default_tid = sys_time_register_timer_offset(modules_sensors_tid, DEFAULT_OFFSET, NULL); // should it be an offset ?
+  modules_sensors_tid = sys_time_register_timer(SENSORS_PERIOD, NULL);
+
+  // common GNC group (estimation, control, actuators, default)
+  // is called with an offset of half the main period (1/PERIODIC_FREQUENCY)
+  // which is the default resolution of SYS_TIME_FREQUENCY,
+  // hence the resolution of the virtual timers.
+  // In practice, this is the best compromised between having enough time between
+  // the sensor readings (triggerd in sensors task group) and the lag between
+  // the state update and control/actuators update
+  //
+  //      |      PERIODIC_FREQ       |
+  //      |            |             |
+  //      read         gnc
+  //
+  modules_gnc_tid = sys_time_register_timer_offset(modules_sensors_tid, 1.f / (2.f * PERIODIC_FREQUENCY), NULL);
 
   // register the timers for the periodic functions
   modules_mcu_core_tid = sys_time_register_timer(SYS_PERIOD, NULL);
@@ -146,38 +139,29 @@ void handle_periodic_tasks(void)
     modules_sensors_periodic_task();
   }
 
-  if (sys_time_check_and_ack_timer(modules_estimation_tid)) {
-    modules_estimation_periodic_task();
-  }
-
   if (sys_time_check_and_ack_timer(modules_radio_control_tid)) {
     radio_control_periodic_task();
     modules_radio_control_periodic_task(); // FIXME integrate above
   }
 
-  if (sys_time_check_and_ack_timer(modules_control_actuators_tid)) {
+  if (sys_time_check_and_ack_timer(modules_gnc_tid)) {
+    modules_estimation_periodic_task();
     modules_control_periodic_task();
-
+    modules_default_periodic_task();
 #if USE_THROTTLE_CURVES
     throttle_curve_run(commands, autopilot_get_mode());
 #endif
-
 #ifndef INTER_MCU_AP
     SetActuatorsFromCommands(commands, autopilot_get_mode());
 #else
     intermcu_set_actuators(commands, autopilot_get_mode());
 #endif
     modules_actuators_periodic_task(); // FIXME integrate above in actuators periodic
-
     if (autopilot_in_flight()) {
       RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++); // TODO make it 1Hz periodic ?
     }
   }
 
-  if (sys_time_check_and_ack_timer(modules_default_tid)) {
-    modules_default_periodic_task();
-  }
-
   if (sys_time_check_and_ack_timer(modules_mcu_core_tid)) {
     modules_mcu_periodic_task();
     modules_core_periodic_task();

sw/airborne/firmwares/rotorcraft/main_chibios.c

@@ -29,9 +29,7 @@
 
 #ifndef  SYS_TIME_FREQUENCY
 #error SYS_TIME_FREQUENCY should be defined in Makefile.chibios or airframe.xml and be equal to CH_CFG_ST_FREQUENCY
-#elif SYS_TIME_FREQUENCY != CH_CFG_ST_FREQUENCY
-#error SYS_TIME_FREQUENCY should be equal to CH_CFG_ST_FREQUENCY
-#elif  CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY)
+#elif  CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY) && SYS_TIME_FREQUENCY < (2 * PERIODIC_FREQUENCY)
 #error CH_CFG_ST_FREQUENCY and SYS_TIME_FREQUENCY should be >= 2 x PERIODIC_FREQUENCY
 #endif
 
@@ -78,16 +76,14 @@ static void thd_ap(void *arg)
   chRegSetThreadName("AP");
 
   while (!chThdShouldTerminateX()) {
+    systime_t t = chVTGetSystemTime();
     handle_periodic_tasks();
     main_event();
-    // In tick mode, the minimum step is 1e6 / CH_CFG_ST_FREQUENCY
-    // which means that whatever happens, if we do a sleep of this
-    // time step, the next wakeup will be "aligned" and we won't see
-    // jitter. The polling on event will also be as fast as possible
-    // Be careful that in tick-less mode, it will be required to use
-    // the chThdSleepUntil function with a correct computation of the
-    // wakeup time, in particular roll-over should be check.
-    chThdSleepMicroseconds(1000000 / CH_CFG_ST_FREQUENCY);
+    // The sleep time is computed to have a polling interval of
+    // 1e6 / CH_CFG_ST_FREQUENCY. If time is passed, thanks to the
+    // "Windowed" sleep function, the execution is not blocked until
+    // a complet roll-over.
+    chThdSleepUntilWindowed(t, t + TIME_US2I(1000000 / CH_CFG_ST_FREQUENCY));
   }
 
   chThdExit(0);

sw/airborne/firmwares/rover/main_ap.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ * Copyright (C) 2018-2021 Gautier Hattenberger <gautier.hattenberger@enac.fr>
  *
  * This file is part of Paparazzi.
  *
@@ -51,11 +51,6 @@ PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
  */
 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
 
-/* MODULES_FREQUENCY is defined in generated/modules.h
- * according to main_freq parameter set for modules in airframe file
- */
-PRINT_CONFIG_VAR(MODULES_FREQUENCY)
-
 #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
@@ -68,28 +63,16 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
  */
 tid_t modules_mcu_core_tid; // single step
 tid_t modules_sensors_tid;
-tid_t modules_estimation_tid;
 tid_t modules_radio_control_tid;
-tid_t modules_control_actuators_tid; // single step
+tid_t modules_gnc_tid; // estimation, control, actuators, default in a single step
 tid_t modules_datalink_tid;
 tid_t modules_default_tid;
 tid_t failsafe_tid;      ///< id for failsafe_check() timer FIXME
 
 #define SYS_PERIOD (1.f / PERIODIC_FREQUENCY)
+#define SENSORS_PERIOD (1.f / PERIODIC_FREQUENCY)
 #define DATALINK_PERIOD (1.f / TELEMETRY_FREQUENCY)
 
-#ifndef ESTIMATION_OFFSET
-#define ESTIMATION_OFFSET 6e-4f
-#endif
-
-#ifndef CONTROL_OFFSET
-#define CONTROL_OFFSET 7e-4f
-#endif
-
-#ifndef DEFAULT_OFFSET
-#define DEFAULT_OFFSET 8e-4f
-#endif
-
 void main_init(void)
 {
   modules_mcu_init();
@@ -108,10 +91,21 @@ void main_init(void)
   autopilot_generated_init();
 
   // register timers with temporal dependencies
-  modules_sensors_tid = sys_time_register_timer(SYS_PERIOD, NULL);
-  modules_estimation_tid = sys_time_register_timer_offset(modules_sensors_tid, ESTIMATION_OFFSET, NULL);
-  modules_control_actuators_tid = sys_time_register_timer_offset(modules_sensors_tid, CONTROL_OFFSET, NULL);
-  modules_default_tid = sys_time_register_timer_offset(modules_sensors_tid, DEFAULT_OFFSET, NULL); // should it be an offset ?
+  modules_sensors_tid = sys_time_register_timer(SENSORS_PERIOD, NULL);
+
+  // common GNC group (estimation, control, actuators, default)
+  // is called with an offset of half the main period (1/PERIODIC_FREQUENCY)
+  // which is the default resolution of SYS_TIME_FREQUENCY,
+  // hence the resolution of the virtual timers.
+  // In practice, this is the best compromised between having enough time between
+  // the sensor readings (triggerd in sensors task group) and the lag between
+  // the state update and control/actuators update
+  //
+  //      |      PERIODIC_FREQ       |
+  //      |            |             |
+  //      read         gnc
+  //
+  modules_gnc_tid = sys_time_register_timer_offset(modules_sensors_tid, 1.f / (2.f * PERIODIC_FREQUENCY), NULL);
 
   // register the timers for the periodic functions
   modules_mcu_core_tid = sys_time_register_timer(SYS_PERIOD, NULL);
@@ -135,29 +129,22 @@ void handle_periodic_tasks(void)
     modules_sensors_periodic_task();
   }
 
-  if (sys_time_check_and_ack_timer(modules_estimation_tid)) {
-    modules_estimation_periodic_task();
-  }
-
   if (sys_time_check_and_ack_timer(modules_radio_control_tid)) {
     radio_control_periodic_task();
     modules_radio_control_periodic_task(); // FIXME integrate above
   }
 
-  if (sys_time_check_and_ack_timer(modules_control_actuators_tid)) {
+  if (sys_time_check_and_ack_timer(modules_gnc_tid)) {
+    modules_estimation_periodic_task();
     modules_control_periodic_task();
+    modules_default_periodic_task();
     SetActuatorsFromCommands(commands, autopilot_get_mode());
     modules_actuators_periodic_task(); // FIXME integrate above in actuators periodic
-
     if (autopilot_in_flight()) {
       RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++); // TODO make it 1Hz periodic ?
     }
   }
 
-  if (sys_time_check_and_ack_timer(modules_default_tid)) {
-    modules_default_periodic_task();
-  }
-
   if (sys_time_check_and_ack_timer(modules_mcu_core_tid)) {
     modules_mcu_periodic_task();
     modules_core_periodic_task();

sw/airborne/firmwares/rover/main_chibios.c

@@ -28,8 +28,6 @@
 
 #ifndef  SYS_TIME_FREQUENCY
 #error SYS_TIME_FREQUENCY should be defined in Makefile.chibios or airframe.xml and be equal to CH_CFG_ST_FREQUENCY
-#elif SYS_TIME_FREQUENCY != CH_CFG_ST_FREQUENCY
-#error SYS_TIME_FREQUENCY should be equal to CH_CFG_ST_FREQUENCY
 #elif  CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY)
 #error CH_CFG_ST_FREQUENCY and SYS_TIME_FREQUENCY should be >= 2 x PERIODIC_FREQUENCY
 #endif
@@ -79,9 +77,14 @@ static void thd_ap(void *arg)
   chRegSetThreadName("AP");
 
   while (!chThdShouldTerminateX()) {
+    systime_t t = chVTGetSystemTime();
     handle_periodic_tasks();
     main_event();
-    chThdSleepMicroseconds(500);
+    // The sleep time is computed to have a polling interval of
+    // 1e6 / CH_CFG_ST_FREQUENCY. If time is passed, thanks to the
+    // "Windowed" sleep function, the execution is not blocked until
+    // a complet roll-over.
+    chThdSleepUntilWindowed(t, t + TIME_US2I(1000000 / CH_CFG_ST_FREQUENCY));
   }
 
   chThdExit(0);