diff --git a/conf/modules/autopilot_gnc_fw.xml b/conf/modules/autopilot_gnc_fw.xml
new file mode 100644
index 0000000000..17a88c6149
--- /dev/null
+++ b/conf/modules/autopilot_gnc_fw.xml
@@ -0,0 +1,16 @@
+
+
+
+
+
+ Autopilot GNC specific to fixedwing part
+
+
+
+ autopilot_gnc
+
+
+
+
+
+
diff --git a/conf/modules/baro_board.xml b/conf/modules/baro_board.xml
index b61ed01659..944a5128b8 100644
--- a/conf/modules/baro_board.xml
+++ b/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)
diff --git a/conf/modules/baro_board_common.xml b/conf/modules/baro_board_common.xml
index 40c4d0bf62..848cd59e4b 100644
--- a/conf/modules/baro_board_common.xml
+++ b/conf/modules/baro_board_common.xml
@@ -20,7 +20,6 @@
-
diff --git a/conf/modules/firmwares/fixedwing.xml b/conf/modules/firmwares/fixedwing.xml
index fe5d058da1..44d62dfeb2 100644
--- a/conf/modules/firmwares/fixedwing.xml
+++ b/conf/modules/firmwares/fixedwing.xml
@@ -8,7 +8,7 @@
- system_core,autopilot_gnc,actuators_pwm
+ system_core,autopilot_gnc_fw,actuators_pwm
diff --git a/conf/modules/intermcu_uart.xml b/conf/modules/intermcu_uart.xml
index 088f473367..0943c5f06a 100644
--- a/conf/modules/intermcu_uart.xml
+++ b/conf/modules/intermcu_uart.xml
@@ -28,9 +28,11 @@
+
+
diff --git a/conf/modules/targets/ap.xml b/conf/modules/targets/ap.xml
index 960ae04f9d..d709e22421 100644
--- a/conf/modules/targets/ap.xml
+++ b/conf/modules/targets/ap.xml
@@ -5,7 +5,7 @@
AP target specific module
- system_core,electrical,settings
+ system_core,electrical,settings,baro_board
diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c
index 3b0bcbef77..6e276d5dfb 100644
--- a/sw/airborne/arch/sim/sim_ap.c
+++ b/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;
diff --git a/sw/airborne/firmwares/fixedwing/autopilot_static.c b/sw/airborne/firmwares/fixedwing/autopilot_static.c
index 5087e94a35..312a8a0d7d 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot_static.c
+++ b/sw/airborne/firmwares/fixedwing/autopilot_static.c
@@ -103,7 +103,6 @@ void autopilot_static_init(void)
void autopilot_static_periodic(void)
{
- attitude_loop();
}
/**
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index d9db08f32e..f289388885 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/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() */
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.h b/sw/airborne/firmwares/fixedwing/main_ap.h
index 687bfedaf3..54db59d5f2 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.h
+++ b/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);
diff --git a/sw/airborne/firmwares/rotorcraft/main_ap.c b/sw/airborne/firmwares/rotorcraft/main_ap.c
index 135a60c733..c8ce6f1ff8 100644
--- a/sw/airborne/firmwares/rotorcraft/main_ap.c
+++ b/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);
diff --git a/sw/airborne/modules/sensors/baro_board_common.c b/sw/airborne/modules/sensors/baro_board_common.c
index 92da5b1f18..7c6b3e323f 100644
--- a/sw/airborne/modules/sensors/baro_board_common.c
+++ b/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)
diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml
index 5259aba9f2..2c85f3ea4a 100644
--- a/sw/simulator/sitl.ml
+++ b/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 *)