[autopilot] convert fixedwing firmware to new AP modules

also load baro_board with ap target as a workaround until relevant
modules are loaded from board XML modules

conf/modules/autopilot_gnc_fw.xml

@@ -0,0 +1,16 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="autopilot_gnc_fw" task="control">
+  <doc>
+    <description>
+      Autopilot GNC specific to fixedwing part
+    </description>
+  </doc>
+  <dep>
+    <depends>autopilot_gnc</depends>
+  </dep>
+  <periodic fun="navigation_task()" freq="NAVIGATION_FREQUENCY" cond="FIXEDWING_FIRMWARE && !USE_GENERATED_AUTOPILOT"/>
+  <periodic fun="attitude_loop()" freq="CONTROL_FREQUENCY" cond="FIXEDWING_FIRMWARE && !USE_GENERATED_AUTOPILOT && !AHRS_TRIGGERED_ATTITUDE_LOOP"/>
+  <makefile target="!fbw"/>
+</module>
+

conf/modules/baro_board.xml

@@ -193,20 +193,6 @@ else ifeq ($(BOARD),$(filter $(BOARD),px4fmu))
     BARO_BOARD_CFLAGS += -DBB_MS5611_SPI_DEV=spi4
     BARO_BOARD_CFLAGS += -DBB_MS5611_SLAVE_IDX=SPI_SLAVE4
   endif
-# apogee baro
-else ifeq ($(BOARD), apogee)
-  BARO_BOARD_CFLAGS += -DUSE_I2C1
-  BARO_BOARD_SRCS += peripherals/mpl3115.c
-  BARO_BOARD_SRCS += $(SRC_BOARD)/baro_board.c
-
-# Umarim
-else ifeq ($(BOARD), umarim)
-  ifeq ($(BOARD_VERSION), 1.0)
-    BARO_BOARD_CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1
-    BARO_BOARD_CFLAGS += -DADS1114_I2C_DEV=i2c1
-    BARO_BOARD_SRCS += peripherals/ads1114.c
-    BARO_BOARD_SRCS += boards/umarim/baro_board.c
-  endif
 
 # Naze32
 else ifeq ($(BOARD), naze32)

conf/modules/baro_board_common.xml

@@ -20,7 +20,6 @@
     <configure name="USE_BARO_BOARD" default="TRUE"/>
     <configure name="BARO_LED" default="none"/>
     <configure name="BARO_PERIODIC_FREQUENCY" default="50"/>
-    <define name="USE_BARO_BOARD" value="$(USE_BARO_BOARD)"/>
     <define name="BARO_LED" value="$(BARO_LED)" cond="ifneq ($(BARO_LED),none)"/>
     <define name="BARO_PERIODIC_FREQUENCY" value="$(BARO_PERIODIC_FREQUENCY)"/>
     <file name="baro_board_common.c"/>

conf/modules/firmwares/fixedwing.xml

@@ -8,7 +8,7 @@
     <configure name="WIND_INFO" value="FALSE|TRUE" description="parse WIND_INFO message (default TRUE)"/>
   </doc>
   <dep>
-    <depends>system_core,autopilot_gnc,actuators_pwm</depends>
+    <depends>system_core,autopilot_gnc_fw,actuators_pwm</depends>
   </dep>
   <makefile>
     <configure name="PERIODIC_FREQUENCY" default="60"/>

conf/modules/intermcu_uart.xml

@@ -28,9 +28,11 @@
     <configure name="INTERMCU_PORT" default="UART2" case="upper|lower"/>
     <configure name="FBW_MODE_LED" default="none"/>
     <define name="FBW_MODE_LED" value="$(FBW_MODE_LED)" cond="ifneq ($(FBW_MODE_LED),none)"/>
+    <define name="INTER_MCU_FBW"/>
   </makefile>
   <makefile target="ap" firmware="fixedwing">
     <configure name="INTERMCU_PORT" default="UART5" case="upper|lower"/>
+    <define name="INTER_MCU_AP"/>
   </makefile>
   <makefile target="ap|fbw" firmware="fixedwing">
     <raw>

conf/modules/targets/ap.xml

@@ -5,7 +5,7 @@
     <description>AP target specific module</description>
   </doc>
   <dep>
-    <depends>system_core,electrical,settings</depends>
+    <depends>system_core,electrical,settings,baro_board</depends>
   </dep>
   <makefile target="ap">
     <define name="AP"/>

sw/airborne/arch/sim/sim_ap.c

@@ -64,36 +64,12 @@ value sim_sys_time_task(value unit)
 
 value sim_periodic_task(value unit)
 {
-  sensors_task();
-#if USE_GENERATED_AUTOPILOT
-  autopilot_periodic();
-#else
-  attitude_loop();
-#endif
-  reporting_task();
-  modules_periodic_task();
-  periodic_task_fbw();
-  electrical_periodic();
+  handle_periodic_tasks_ap();
   event_task_ap();
   event_task_fbw();
   return unit;
 }
 
-value sim_monitor_task(value unit)
-{
-  monitor_task();
-  return unit;
-}
-
-value sim_nav_task(value unit)
-{
-#if !USE_GENERATED_AUTOPILOT
-  navigation_task();
-#endif
-  return unit;
-}
-
-
 float ftimeofday(void)
 {
   struct timeval t;

sw/airborne/firmwares/fixedwing/autopilot_static.c

@@ -103,7 +103,6 @@ void autopilot_static_init(void)
 
 void autopilot_static_periodic(void)
 {
-  attitude_loop();
 }
 
 /**

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -40,31 +40,9 @@
 #include "inter_mcu.h"
 #include "link_mcu.h"
 
-// Sensors
-#if USE_GPS
-#include "subsystems/gps.h"
-#endif
-#if USE_IMU
-#include "subsystems/imu.h"
-#endif
-
-// autopilot
-#include "autopilot.h"
-#include "firmwares/fixedwing/nav.h"
 #include "generated/flight_plan.h"
-
-// datalink & telemetry
-#if PERIODIC_TELEMETRY
-#include "subsystems/datalink/telemetry.h"
-#endif
-
-// modules & settings
-#include "subsystems/settings.h"
+#include "generated/airframe.h"
 #include "generated/modules.h"
-#include "generated/settings.h"
-#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
-#include "modules/settings/rc_settings.h"
-#endif
 #include "subsystems/abi.h"
 
 #include "led.h"
@@ -101,11 +79,6 @@ PRINT_CONFIG_VAR(CONTROL_FREQUENCY)
 #endif
 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_IMU
 #ifdef AHRS_PROPAGATE_FREQUENCY
@@ -116,33 +89,47 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
 #endif
 #endif // USE_IMU
 
+/**
+ * IDs for timers
+ */
+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_datalink_tid;
+tid_t modules_default_tid;
+tid_t monitor_tid;     ///< id for monitor_task() timer FIXME
 
-tid_t modules_tid;     ///< id for modules_periodic_task() timer
-tid_t telemetry_tid;   ///< id for telemetry_periodic() timer
-tid_t sensors_tid;     ///< id for sensors_task() timer
-tid_t attitude_tid;    ///< id for attitude_loop() timer
-#if !USE_GENERATED_AUTOPILOT
-tid_t navigation_tid;  ///< id for navigation_task() timer
+#define SYS_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
-tid_t monitor_tid;     ///< id for monitor_task() timer
 
 void init_ap(void)
 {
-#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
-  mcu_init();
-#endif /* SINGLE_MCU */
-
-  /************* Sensors initialization ***************/
-
-  /************* Links initialization ***************/
-#if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
-  link_mcu_init();
+#ifndef SINGLE_MCU
+  modules_mcu_init();
 #endif
-
-  /************ Internal status ***************/
-  autopilot_init();
-
-  modules_init();
+  modules_core_init();
+  modules_sensors_init();
+  modules_estimation_init();
+  //radio_control_init(); FIXME done in FBW
+  // modules_radio_control_init(); FIXME
+  modules_control_init();
+  modules_actuators_init();
+  modules_datalink_init();
+  modules_default_init();
 
   // call autopilot implementation init after guidance modules init
   // it will set startup mode
@@ -152,21 +139,17 @@ void init_ap(void)
   autopilot_static_init();
 #endif
 
-  settings_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 ?
 
-  /**** start timers for periodic functions *****/
-  sensors_tid = sys_time_register_timer(1. / PERIODIC_FREQUENCY, NULL);
-#if !USE_GENERATED_AUTOPILOT
-  navigation_tid = sys_time_register_timer(1. / NAVIGATION_FREQUENCY, NULL);
-#endif
-  attitude_tid = sys_time_register_timer(1. / CONTROL_FREQUENCY, NULL);
-  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
-  telemetry_tid = sys_time_register_timer(1. / TELEMETRY_FREQUENCY, NULL);
-  monitor_tid = sys_time_register_timer(1.0, NULL);
-
-#if DOWNLINK
-  downlink_init();
-#endif
+  // 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.
@@ -186,42 +169,41 @@ void init_ap(void)
 
 void handle_periodic_tasks_ap(void)
 {
-
-  if (sys_time_check_and_ack_timer(sensors_tid)) {
-    sensors_task();
+  if (sys_time_check_and_ack_timer(modules_sensors_tid)) {
+    modules_sensors_periodic_task();
   }
 
-#if USE_GENERATED_AUTOPILOT
-  if (sys_time_check_and_ack_timer(attitude_tid)) {
-    autopilot_periodic();
-  }
-#else
-  // static autopilot
-  if (sys_time_check_and_ack_timer(navigation_tid)) {
-    navigation_task();
+  if (sys_time_check_and_ack_timer(modules_estimation_tid)) {
+    modules_estimation_periodic_task();
   }
 
-#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
-  if (sys_time_check_and_ack_timer(attitude_tid)) {
-    attitude_loop();
-  }
-#endif
+  // 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
+  //}
 
-#endif
-
-  if (sys_time_check_and_ack_timer(modules_tid)) {
-    modules_periodic_task();
+  if (sys_time_check_and_ack_timer(modules_control_actuators_tid)) {
+    modules_control_periodic_task();
   }
 
-  if (sys_time_check_and_ack_timer(monitor_tid)) {
-    monitor_task();
+  if (sys_time_check_and_ack_timer(modules_default_tid)) {
+    modules_default_periodic_task();
   }
 
-  if (sys_time_check_and_ack_timer(telemetry_tid)) {
+  if (sys_time_check_and_ack_timer(modules_mcu_core_tid)) {
+    modules_mcu_periodic_task();
+    modules_core_periodic_task();
+    LED_PERIODIC(); // FIXME periodic in led module
+  }
+
+  if (sys_time_check_and_ack_timer(modules_datalink_tid)) {
     reporting_task();
-    LED_PERIODIC();
+    modules_datalink_periodic_task(); // FIXME integrate above
+#if defined DATALINK || defined SITL
+    RunOnceEvery(TELEMETRY_FREQUENCY, datalink_time++);
+#endif
   }
-
 }
 
 
@@ -230,7 +212,6 @@ void handle_periodic_tasks_ap(void)
 
 /**
  * Send a series of initialisation messages followed by a stream of periodic ones.
- * Called at 60Hz.
  */
 void reporting_task(void)
 {
@@ -245,7 +226,6 @@ void reporting_task(void)
   }
   /* then report periodicly */
   else {
-    //PeriodicSendAp(DefaultChannel, DefaultDevice);
 #if PERIODIC_TELEMETRY
     periodic_telemetry_send_Ap(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
 #endif
@@ -253,16 +233,6 @@ void reporting_task(void)
 }
 
 
-
-/** Run at PERIODIC_FREQUENCY (60Hz if not defined) */
-void sensors_task(void)
-{
-  //FIXME: this is just a kludge
-#if USE_AHRS && defined SITL && !USE_NPS
-  update_ahrs_from_sim();
-#endif
-}
-
 #ifdef LOW_BATTERY_KILL_DELAY
 #warning LOW_BATTERY_KILL_DELAY has been renamed to CATASTROPHIC_BAT_KILL_DELAY, please update your airframe file!
 #endif
@@ -288,9 +258,6 @@ void monitor_task(void)
   if (autopilot.flight_time) {
     autopilot.flight_time++;
   }
-#if defined DATALINK || defined SITL
-  datalink_time++;
-#endif
 
   static uint8_t t = 0;
   if (ap_electrical.vsupply < CATASTROPHIC_BAT_LEVEL) {
@@ -320,26 +287,27 @@ void monitor_task(void)
 /*********** EVENT ***********************************************************/
 void event_task_ap(void)
 {
-
 #ifndef SINGLE_MCU
   /* for SINGLE_MCU done in main_fbw */
   /* event functions for mcu peripherals: i2c, usb_serial.. */
-  mcu_event();
+  modules_mcu_event_task();
 #endif /* SINGLE_MCU */
+  modules_core_event_task();
+  modules_sensors_event_task();
+  modules_estimation_event_task();
+  modules_datalink_event_task();
+  modules_default_event_task();
 
-  modules_event_task();
 
+  // TODO integrate in modules
 #if defined MCU_SPI_LINK || defined MCU_UART_LINK
   link_mcu_event_task();
 #endif
-
   if (inter_mcu_received_fbw) {
     /* receive radio control task from fbw */
     inter_mcu_received_fbw = false;
     autopilot_on_rc_frame();
   }
 
-  autopilot_event();
-
 } /* event_task_ap() */
 

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 sensors_task(void);
 extern void monitor_task(void);
 extern void reporting_task(void);
 

sw/airborne/firmwares/rotorcraft/main_ap.c

@@ -250,7 +250,7 @@ void failsafe_check(void)
       radio_control.status != RC_OK &&
 #endif
 #ifdef NO_GPS_LOST_WITH_DATALINK_TIME
-      datalink_time > NO_GPS_LOST_WITH_DATALINK_TIME && 
+      datalink_time > NO_GPS_LOST_WITH_DATALINK_TIME &&
 #endif
       GpsIsLost()) {
     autopilot_set_mode(AP_MODE_FAILSAFE);

sw/airborne/modules/sensors/baro_board_common.c

@@ -30,7 +30,7 @@
 #include BOARD_CONFIG
 
 #if USE_BARO_BOARD
-PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD)
+//PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD) // FIXME debug message broken?
 #endif
 
 void baro_board_init(void)

sw/simulator/sitl.ml

@@ -41,8 +41,6 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
 
   let servos_period = 1./.40. (* s *)
   let periodic_period = 1./.60. (* s *)
-  let nav_period = 1./.4. (* s *)
-  let monitor_period = 1. (* s *)
   let rc_period = 1./.40. (* s *)
   let sys_time_period = 1./.120. (* s *)
 
@@ -124,8 +122,6 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
 
   external sys_time_task : unit -> unit = "sim_sys_time_task"
   external periodic_task : unit -> unit = "sim_periodic_task"
-  external nav_task : unit -> unit = "sim_nav_task"
-  external monitor_task : unit -> unit = "sim_monitor_task"
   external sim_init : unit -> unit = "sim_init"
   external update_bat : int -> unit = "update_bat"
   external update_adc1 : int -> unit = "update_adc1"
@@ -191,8 +187,6 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
   let boot = fun time_scale ->
     Simlib.timer ~scale:time_scale servos_period (update_servos bat_button);
     Simlib.timer ~scale:time_scale periodic_period periodic_task;
-    Simlib.timer ~scale:time_scale nav_period nav_task;
-    Simlib.timer ~scale:time_scale monitor_period monitor_task;
     Simlib.timer ~scale:time_scale sys_time_period sys_time_task;
 
     (* Forward or broacast messages according to "link" mode *)