diff --git a/conf/abi.xml b/conf/abi.xml index f178413464..7b8b360d41 100644 --- a/conf/abi.xml +++ b/conf/abi.xml @@ -211,6 +211,10 @@ Pointer to an array of bytes, or any structure depending on the sender and the type of data + + Pointer to a radio control structure + + diff --git a/conf/airframes/examples/separate_fbw_ap.xml b/conf/airframes/examples/separate_fbw_ap.xml index 35a0ce66a6..09a651fc84 100644 --- a/conf/airframes/examples/separate_fbw_ap.xml +++ b/conf/airframes/examples/separate_fbw_ap.xml @@ -205,8 +205,6 @@ - - @@ -227,6 +225,17 @@ + + + + + + @@ -234,38 +243,43 @@ - - - - - + + + + + + + + + + - - - + + + - @@ -293,16 +307,6 @@ - - - - - - - - - - diff --git a/conf/autopilot/rotorcraft_autopilot.xml b/conf/autopilot/rotorcraft_autopilot.xml index 9adfd570bd..55b578f59b 100644 --- a/conf/autopilot/rotorcraft_autopilot.xml +++ b/conf/autopilot/rotorcraft_autopilot.xml @@ -28,9 +28,8 @@ - + - @@ -66,7 +65,7 @@ - + @@ -83,7 +82,7 @@ - + @@ -103,7 +102,7 @@ - + @@ -124,7 +123,7 @@ - + @@ -143,7 +142,7 @@ - + @@ -160,7 +159,7 @@ - + diff --git a/conf/autopilot/rover.xml b/conf/autopilot/rover.xml index f9a4d8a06d..6f47a139a0 100644 --- a/conf/autopilot/rover.xml +++ b/conf/autopilot/rover.xml @@ -32,7 +32,6 @@ - @@ -44,7 +43,6 @@ - @@ -55,7 +53,6 @@ - diff --git a/conf/autopilot/rover_holonomic.xml b/conf/autopilot/rover_holonomic.xml index 264b07ecbb..7ce6c435b0 100644 --- a/conf/autopilot/rover_holonomic.xml +++ b/conf/autopilot/rover_holonomic.xml @@ -32,7 +32,6 @@ - @@ -45,7 +44,6 @@ - @@ -56,7 +54,6 @@ - diff --git a/conf/boards/apogee_1.0_chibios.makefile b/conf/boards/apogee_1.0_chibios.makefile index 6b36d6cdc6..7c5e694171 100644 --- a/conf/boards/apogee_1.0_chibios.makefile +++ b/conf/boards/apogee_1.0_chibios.makefile @@ -17,7 +17,7 @@ RTOS=chibios ## FPU on F4 USE_FPU=softfp -$(TARGET).CFLAGS += -DSTM32F4 -DPPRZLINK_ENABLE_FD -DUSE_HARD_FAULT_RECOVERY +$(TARGET).CFLAGS += -DSTM32F4 -DPPRZLINK_ENABLE_FD ############################################################################## # Architecture or project specific options diff --git a/conf/boards/chimera_1.0.makefile b/conf/boards/chimera_1.0.makefile index 92ad488d8c..e64127e800 100644 --- a/conf/boards/chimera_1.0.makefile +++ b/conf/boards/chimera_1.0.makefile @@ -23,7 +23,7 @@ USE_FPU_OPT= -mfpu=fpv5-d16 USE_LTO ?= yes -$(TARGET).CFLAGS += -DSTM32F7 -DPPRZLINK_ENABLE_FD -DUSE_HARD_FAULT_RECOVERY +$(TARGET).CFLAGS += -DSTM32F7 -DPPRZLINK_ENABLE_FD ############################################################################## # Architecture or project specific options diff --git a/conf/boards/crazyflie_2.1.makefile b/conf/boards/crazyflie_2.1.makefile index 33c33f6971..b968de9ad5 100644 --- a/conf/boards/crazyflie_2.1.makefile +++ b/conf/boards/crazyflie_2.1.makefile @@ -18,7 +18,7 @@ ARCH=chibios ## FPU on F4 USE_FPU=softfp -$(TARGET).CFLAGS += -DSTM32F4 -DPPRZLINK_ENABLE_FD -DUSE_HARD_FAULT_RECOVERY +$(TARGET).CFLAGS += -DSTM32F4 -DPPRZLINK_ENABLE_FD ############################################################################## # Architecture or project specific options diff --git a/conf/boards/holybro_kakute_f7.makefile b/conf/boards/holybro_kakute_f7.makefile index 7cdbb3c299..433531c421 100644 --- a/conf/boards/holybro_kakute_f7.makefile +++ b/conf/boards/holybro_kakute_f7.makefile @@ -24,7 +24,6 @@ USE_FPU_OPT= -mfpu=fpv5-sp-d16 -fsingle-precision-constant USE_LTO ?= yes $(TARGET).CFLAGS += -DSTM32F7 -DPPRZLINK_ENABLE_FD -DDSHOT_CHANNEL_FIRST_INDEX=1U -#$(TARGET).CFLAGS += -DUSE_HARD_FAULT_RECOVERY ############################################################################## # Architecture or project specific options diff --git a/conf/boards/matek_f765_wing.makefile b/conf/boards/matek_f765_wing.makefile index a35654370c..b0f5b5994b 100644 --- a/conf/boards/matek_f765_wing.makefile +++ b/conf/boards/matek_f765_wing.makefile @@ -23,7 +23,7 @@ USE_FPU_OPT= -mfpu=fpv5-d16 USE_LTO ?= yes -$(TARGET).CFLAGS += -DSTM32F7 -DPPRZLINK_ENABLE_FD -DUSE_HARD_FAULT_RECOVERY -DDSHOT_CHANNEL_FIRST_INDEX=1U +$(TARGET).CFLAGS += -DSTM32F7 -DPPRZLINK_ENABLE_FD -DDSHOT_CHANNEL_FIRST_INDEX=1U ############################################################################## # Architecture or project specific options diff --git a/conf/boards/nucleo144_f767zi.makefile b/conf/boards/nucleo144_f767zi.makefile index 97488596b7..21acec3848 100644 --- a/conf/boards/nucleo144_f767zi.makefile +++ b/conf/boards/nucleo144_f767zi.makefile @@ -23,7 +23,7 @@ USE_FPU_OPT= -mfpu=fpv5-d16 USE_LTO=yes -$(TARGET).CFLAGS += -DSTM32F7 -DPPRZLINK_ENABLE_FD -DUSE_HARD_FAULT_RECOVERY +$(TARGET).CFLAGS += -DSTM32F7 -DPPRZLINK_ENABLE_FD ############################################################################## # Architecture or project specific options diff --git a/conf/boards/tawaki_common.makefile b/conf/boards/tawaki_common.makefile index b945573a39..e0a6842c81 100644 --- a/conf/boards/tawaki_common.makefile +++ b/conf/boards/tawaki_common.makefile @@ -22,7 +22,7 @@ USE_FPU_OPT= -mfpu=fpv5-d16 USE_LTO ?= yes -$(TARGET).CFLAGS += -DSTM32F7 -DPPRZLINK_ENABLE_FD -DUSE_HARD_FAULT_RECOVERY -DDSHOT_CHANNEL_FIRST_INDEX=1U +$(TARGET).CFLAGS += -DSTM32F7 -DPPRZLINK_ENABLE_FD -DDSHOT_CHANNEL_FIRST_INDEX=1U ############################################################################## # Architecture or project specific options diff --git a/conf/modules/actuators.xml b/conf/modules/actuators.xml index f4f35b7e5b..aff78576c8 100644 --- a/conf/modules/actuators.xml +++ b/conf/modules/actuators.xml @@ -12,6 +12,7 @@ + diff --git a/conf/modules/actuators_dummy.xml b/conf/modules/actuators_dummy.xml index 407a3fc87f..b38e85fa88 100644 --- a/conf/modules/actuators_dummy.xml +++ b/conf/modules/actuators_dummy.xml @@ -10,6 +10,7 @@ + actuators
diff --git a/conf/modules/boards/apogee_1.0_chibios.xml b/conf/modules/boards/apogee_1.0_chibios.xml index c17b4a7493..330e675fdd 100644 --- a/conf/modules/boards/apogee_1.0_chibios.xml +++ b/conf/modules/boards/apogee_1.0_chibios.xml @@ -7,7 +7,7 @@ - i2c,baro_board_common + i2c,baro_board_common,hard_fault_recovery baro diff --git a/conf/modules/boards/chimera_1.0.xml b/conf/modules/boards/chimera_1.0.xml index 9c131c5cbb..973531ff3d 100644 --- a/conf/modules/boards/chimera_1.0.xml +++ b/conf/modules/boards/chimera_1.0.xml @@ -7,7 +7,7 @@ - i2c,baro_board + i2c,baro_board,hard_fault_recovery diff --git a/conf/modules/boards/crazyflie_2.1.xml b/conf/modules/boards/crazyflie_2.1.xml new file mode 100644 index 0000000000..d99f1b387c --- /dev/null +++ b/conf/modules/boards/crazyflie_2.1.xml @@ -0,0 +1,11 @@ + + + + + + Specific configuration for Crazyflie 2.1 + + + + + diff --git a/conf/modules/boards/matek_f765_wing.xml b/conf/modules/boards/matek_f765_wing.xml new file mode 100644 index 0000000000..462cedf90d --- /dev/null +++ b/conf/modules/boards/matek_f765_wing.xml @@ -0,0 +1,14 @@ + + + + + + Specific configuration for Matek F765 Wing + + + + hard_fault_recovery + + + + diff --git a/conf/modules/boards/nucleo144_f767zi.xml b/conf/modules/boards/nucleo144_f767zi.xml new file mode 100644 index 0000000000..50fa22e2df --- /dev/null +++ b/conf/modules/boards/nucleo144_f767zi.xml @@ -0,0 +1,14 @@ + + + + + + Specific configuration for Nucleo144 F767zi dev board + + + + hard_fault_recovery + + + + diff --git a/conf/modules/boards/tawaki_1.0.xml b/conf/modules/boards/tawaki_1.0.xml new file mode 100644 index 0000000000..2eb924ab9a --- /dev/null +++ b/conf/modules/boards/tawaki_1.0.xml @@ -0,0 +1,14 @@ + + + + + + Specific configuration for Tawaki 1.0 + + + + hard_fault_recovery + + + + diff --git a/conf/modules/boards/tawaki_1.1.xml b/conf/modules/boards/tawaki_1.1.xml new file mode 100644 index 0000000000..7c0e88bbf1 --- /dev/null +++ b/conf/modules/boards/tawaki_1.1.xml @@ -0,0 +1,14 @@ + + + + + + Specific configuration for Tawaki 1.1 + + + + hard_fault_recovery + + + + diff --git a/conf/modules/datalink_common.xml b/conf/modules/datalink_common.xml index dd6797b6cf..9010d4db1d 100644 --- a/conf/modules/datalink_common.xml +++ b/conf/modules/datalink_common.xml @@ -12,6 +12,8 @@
+ + diff --git a/conf/modules/distributed_circular_formation.xml b/conf/modules/distributed_circular_formation.xml index fe165cf544..8f0646b1b0 100644 --- a/conf/modules/distributed_circular_formation.xml +++ b/conf/modules/distributed_circular_formation.xml @@ -38,7 +38,7 @@ - + diff --git a/conf/modules/firmwares/fixedwing.xml b/conf/modules/firmwares/fixedwing.xml index cf869db8ed..f3c7a61441 100644 --- a/conf/modules/firmwares/fixedwing.xml +++ b/conf/modules/firmwares/fixedwing.xml @@ -21,28 +21,11 @@ - - - - - - + + - - - - - - - - - - - - - - + diff --git a/conf/modules/firmwares/rotorcraft.xml b/conf/modules/firmwares/rotorcraft.xml index 6f2bc0e763..4f7a79983c 100644 --- a/conf/modules/firmwares/rotorcraft.xml +++ b/conf/modules/firmwares/rotorcraft.xml @@ -18,14 +18,11 @@ - - + + - - - - + diff --git a/conf/modules/firmwares/rover.xml b/conf/modules/firmwares/rover.xml index bc7f4b5367..a7d83ee0fd 100644 --- a/conf/modules/firmwares/rover.xml +++ b/conf/modules/firmwares/rover.xml @@ -18,9 +18,9 @@ - - - + + + ifneq ($(USE_GENERATED_AUTOPILOT), TRUE) $(error "Rover firmware should use generated autopilot") diff --git a/conf/modules/gps_intermcu.xml b/conf/modules/gps_intermcu.xml new file mode 100644 index 0000000000..4f8e13e6d2 --- /dev/null +++ b/conf/modules/gps_intermcu.xml @@ -0,0 +1,47 @@ + + + + + + Remote GPS via intermcu. + Parses the IMCU_REMOTE_GPS message and publishes it onboard via ABI. + + + + gps,@datalink + gps + + + +
+ +
+ + + + + + + ifdef SECONDARY_GPS + ifneq (,$(findstring $(SECONDARY_GPS), intermcu)) + # this is the secondary GPS + $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"modules/gps/gps_intermcu.h\" + $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_IMCU + else + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/gps/gps_intermcu.h\" + $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_IMCU + endif + else + # plain old single GPS usage + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/gps/gps_intermcu.h\" + endif + + + + + + + + + + diff --git a/conf/modules/hard_fault_recovery.xml b/conf/modules/hard_fault_recovery.xml new file mode 100644 index 0000000000..54a34ef012 --- /dev/null +++ b/conf/modules/hard_fault_recovery.xml @@ -0,0 +1,31 @@ + + + + + + Minimal autopilot for recovery + + Currently, only allow to take a fixedwing plane in manual in case of MCU hard fault + Recommanded modules are: electrical, radio_control and actuators + + Init, periodic and event functions are called from main_chibios when activated + + + + + system_core + recovery + + + + + + + + + + + + + + diff --git a/conf/modules/intermcu_can.xml b/conf/modules/intermcu_can.xml deleted file mode 100644 index 5fd80d213a..0000000000 --- a/conf/modules/intermcu_can.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - Inter-MCU communication over CAN - - To enable this, please set the "SEPARATE_FBW" configure option to TRUE - in your airframe file for both AP and FBW targets - - - - - - intermcu,commands,radio_control - - - - ifeq (,$(findstring $(SEPARATE_FBW),1 TRUE)) - $(error Using intermcu via CAN, so dual mcu with separate fbw. Please set option "SEPARATE_FBW" to TRUE. - endif - - - - - - - - - - diff --git a/conf/modules/intermcu_spi.xml b/conf/modules/intermcu_spi.xml deleted file mode 100644 index 7b1bec4915..0000000000 --- a/conf/modules/intermcu_spi.xml +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - Inter-MCU communication over SPI - - To enable this, please set the "SEPARATE_FBW" configure option to TRUE - in your airframe file for both AP and FBW targets - - - - - - - spi_master - intermcu,commands,radio_control - - - - - ifeq (,$(findstring $(SEPARATE_FBW),1 TRUE)) - $(error Using intermcu via SPI, so dual mcu with separate fbw. Please set option "SEPARATE_FBW" to TRUE. - endif - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/modules/intermcu_uart.xml b/conf/modules/intermcu_uart.xml index b358283c7e..3ea65134fb 100644 --- a/conf/modules/intermcu_uart.xml +++ b/conf/modules/intermcu_uart.xml @@ -4,78 +4,55 @@ Inter-MCU communication over UART - - To enable this, please set the "SEPARATE_FBW" configure option to TRUE - in your airframe file for both AP and FBW targets - - - uart - intermcu,commands,radio_control + uart,radio_control_intermcu,datalink_common + intermcu,commands
- - + + +
- - - - - - - - - - - - - ifeq (,$(findstring $(SEPARATE_FBW),1 TRUE)) - $(error Using intermcu via UART, so dual mcu with separate fbw. Please set option "SEPARATE_FBW" to TRUE. - endif - - - - - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/mcu.xml b/conf/modules/mcu.xml index d1fb5c698b..731582d9a1 100644 --- a/conf/modules/mcu.xml +++ b/conf/modules/mcu.xml @@ -4,7 +4,6 @@ Automatic initialization module for all MCU peripherals - Also includes GPIO and LED drivers @@ -21,14 +20,9 @@ - - - - - diff --git a/conf/modules/meteo_france_DAQ.xml b/conf/modules/meteo_france_DAQ.xml index cfc39531ea..b3c8dc15ff 100644 --- a/conf/modules/meteo_france_DAQ.xml +++ b/conf/modules/meteo_france_DAQ.xml @@ -20,7 +20,7 @@ - + diff --git a/conf/modules/module.dtd b/conf/modules/module.dtd index f636bdbd46..dc10bc0055 100644 --- a/conf/modules/module.dtd +++ b/conf/modules/module.dtd @@ -63,7 +63,9 @@ cond CDATA #IMPLIED> +fun CDATA #REQUIRED +class CDATA #IMPLIED +cond CDATA #IMPLIED> - spi_master + spi_master,radio_control_common radio_control
@@ -44,19 +44,17 @@
- + + - - - @@ -70,7 +68,6 @@ - diff --git a/conf/modules/radio_control_common.xml b/conf/modules/radio_control_common.xml new file mode 100644 index 0000000000..453115801e --- /dev/null +++ b/conf/modules/radio_control_common.xml @@ -0,0 +1,25 @@ + + + + + + Radio control common + + + +
+ +
+ + + + + + + + + + + + + diff --git a/conf/modules/radio_control_datalink.xml b/conf/modules/radio_control_datalink.xml index 62938556eb..0d9e708309 100644 --- a/conf/modules/radio_control_datalink.xml +++ b/conf/modules/radio_control_datalink.xml @@ -5,37 +5,25 @@ Radio control over datalink - + radio_control_common radio_control
+ + - - - - - - - - - - - - - - diff --git a/conf/modules/radio_control_hott.xml b/conf/modules/radio_control_hott.xml index 6c6be283e2..4461fbbef3 100644 --- a/conf/modules/radio_control_hott.xml +++ b/conf/modules/radio_control_hott.xml @@ -5,45 +5,27 @@ Radio control based on Graupner HOTT SUMD - - uart + uart,radio_control_common radio_control
+ + - - - - - - - - - - - - - - - - - - - diff --git a/conf/modules/radio_control_intermcu.xml b/conf/modules/radio_control_intermcu.xml new file mode 100644 index 0000000000..aae5dd89c7 --- /dev/null +++ b/conf/modules/radio_control_intermcu.xml @@ -0,0 +1,28 @@ + + + + + + Radio control over intermcu + + + + radio_control_common + radio_control + +
+ +
+ + + + + + + + + + + + + diff --git a/conf/modules/radio_control_ppm.xml b/conf/modules/radio_control_ppm.xml index d357948c98..4ec237e601 100644 --- a/conf/modules/radio_control_ppm.xml +++ b/conf/modules/radio_control_ppm.xml @@ -10,38 +10,24 @@ If they set the PPM_CONFIG makefile variable, add it to the target. The PPM_CONFIG define is then used in the _board_.h file to set the configuration. - + radio_control_common radio_control
+ + - - - - - - - - - - - - - - - - diff --git a/conf/modules/radio_control_sbus.xml b/conf/modules/radio_control_sbus.xml index b012082cb9..1e84f02d26 100644 --- a/conf/modules/radio_control_sbus.xml +++ b/conf/modules/radio_control_sbus.xml @@ -5,47 +5,28 @@ Radio control based on Futaba SBUS - - uart + uart,radio_control_common radio_control
+ + - - - - - - - - - - - - - - - - - - - - diff --git a/conf/modules/radio_control_sbus_dual.xml b/conf/modules/radio_control_sbus_dual.xml index 9be8253350..e3f51fbd5c 100644 --- a/conf/modules/radio_control_sbus_dual.xml +++ b/conf/modules/radio_control_sbus_dual.xml @@ -5,26 +5,21 @@ Radio control using two Futaba SBUS receivers - - uart + uart,radio_control_common radio_control
+ + - - - - - - @@ -32,26 +27,11 @@ - - - - - - - - - - - - - - - diff --git a/conf/modules/radio_control_spektrum.xml b/conf/modules/radio_control_spektrum.xml index 77406431c1..cd97626f83 100644 --- a/conf/modules/radio_control_spektrum.xml +++ b/conf/modules/radio_control_spektrum.xml @@ -7,7 +7,6 @@ Define USE_DSMX on STM32 microcontrollers to bind in DSMX instead of DSM2 - @@ -15,19 +14,15 @@ - uart + uart,radio_control_common radio_control
- - - - - - - + + + @@ -45,36 +40,9 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -86,13 +54,9 @@ - - - - diff --git a/conf/modules/radio_control_superbitrf_rc.xml b/conf/modules/radio_control_superbitrf_rc.xml index cf5c9ea7ec..894f361945 100644 --- a/conf/modules/radio_control_superbitrf_rc.xml +++ b/conf/modules/radio_control_superbitrf_rc.xml @@ -5,7 +5,6 @@ Radio control based on the SuperbitRF chip - @@ -20,37 +19,24 @@ - spi_master + spi_master,radio_control_common radio_control
+ + - - - - + - - - - - - - - - - - - @@ -58,7 +44,6 @@ - diff --git a/conf/modules/rssi.xml b/conf/modules/rssi.xml index 6e82086639..937a5e6845 100644 --- a/conf/modules/rssi.xml +++ b/conf/modules/rssi.xml @@ -11,7 +11,7 @@ - + diff --git a/conf/modules/stabilization_adaptive_fw.xml b/conf/modules/stabilization_adaptive_fw.xml index a221986338..97b012bfd6 100644 --- a/conf/modules/stabilization_adaptive_fw.xml +++ b/conf/modules/stabilization_adaptive_fw.xml @@ -33,9 +33,9 @@ - - - + + + diff --git a/conf/modules/stabilization_attitude_fw.xml b/conf/modules/stabilization_attitude_fw.xml index f804123e58..4516e52bc6 100644 --- a/conf/modules/stabilization_attitude_fw.xml +++ b/conf/modules/stabilization_attitude_fw.xml @@ -23,9 +23,9 @@ - - - + + + diff --git a/conf/modules/system_core.xml b/conf/modules/system_core.xml index 558f463223..79a911e28a 100644 --- a/conf/modules/system_core.xml +++ b/conf/modules/system_core.xml @@ -5,13 +5,19 @@ Core system components meta module - Includes: sys_time, commands interfaces + Includes: sys_time, commands, LED interfaces mcu,math,state_interface,@actuators|@intermcu,settings|@no_settings core +
+ + +
+ + @@ -21,7 +27,10 @@ + + + VPATH += $(PAPARAZZI_HOME)/var/share @@ -30,17 +39,8 @@ $(TARGET).ARCHDIR = $(ARCH) - - - - - - - - - - - + + diff --git a/conf/modules/targets/fbw.xml b/conf/modules/targets/fbw.xml index 9f442e7f85..72e9933a01 100644 --- a/conf/modules/targets/fbw.xml +++ b/conf/modules/targets/fbw.xml @@ -5,11 +5,16 @@ FBW target specific module - electrical + electrical,@intermcu no_settings +
+ +
+ + diff --git a/conf/modules/targets/hitl.xml b/conf/modules/targets/hitl.xml index d5d00b4e1a..cc7918ddd8 100644 --- a/conf/modules/targets/hitl.xml +++ b/conf/modules/targets/hitl.xml @@ -15,6 +15,7 @@ + diff --git a/conf/modules/targets/nps.xml b/conf/modules/targets/nps.xml index 718828529a..875e55c580 100644 --- a/conf/modules/targets/nps.xml +++ b/conf/modules/targets/nps.xml @@ -42,6 +42,7 @@ VPATH += $(PAPARAZZI_SRC)/sw/simulator + diff --git a/conf/modules/targets/sim.xml b/conf/modules/targets/sim.xml index d33d2c4715..6569ad4117 100644 --- a/conf/modules/targets/sim.xml +++ b/conf/modules/targets/sim.xml @@ -16,8 +16,7 @@ - - + diff --git a/conf/modules/telemetry_bluegiga.xml b/conf/modules/telemetry_bluegiga.xml index 757567797c..9087ca5e8e 100644 --- a/conf/modules/telemetry_bluegiga.xml +++ b/conf/modules/telemetry_bluegiga.xml @@ -33,7 +33,7 @@ - + diff --git a/conf/modules/telemetry_intermcu.xml b/conf/modules/telemetry_intermcu.xml index ff2e98b5f0..8eced62d53 100644 --- a/conf/modules/telemetry_intermcu.xml +++ b/conf/modules/telemetry_intermcu.xml @@ -1,43 +1,27 @@ - + - Telemetry over InterMCU - This module transmits Telemetry of the process "InterMCU" from AP to FBW. The FBW then transmits this telemetry further through PPRZ over an UART. + + Telemetry bridge over InterMCU for FBW + This module transmits datalink of the process "InterMCU" from AP to FBW. The FBW then transmits this telemetry further through PPRZ over an UART. - - + - uart - + uart,@intermcu datalink,telemetry
- +
- - - - - - + + + + + - - - - - - - - - - - - - - diff --git a/conf/modules/telemetry_secure_common.xml b/conf/modules/telemetry_secure_common.xml index 5c4a22d40c..53bf23399c 100644 --- a/conf/modules/telemetry_secure_common.xml +++ b/conf/modules/telemetry_secure_common.xml @@ -15,7 +15,7 @@ - + diff --git a/conf/modules/telemetry_superbitrf.xml b/conf/modules/telemetry_superbitrf.xml index 14bb98d2ac..d0aa403948 100644 --- a/conf/modules/telemetry_superbitrf.xml +++ b/conf/modules/telemetry_superbitrf.xml @@ -17,7 +17,7 @@ - + diff --git a/conf/modules/telemetry_transparent.xml b/conf/modules/telemetry_transparent.xml index 7a3b995071..7e7a452d03 100644 --- a/conf/modules/telemetry_transparent.xml +++ b/conf/modules/telemetry_transparent.xml @@ -9,6 +9,7 @@ + uart,datalink_common @@ -20,8 +21,8 @@ - - + + diff --git a/conf/modules/telemetry_transparent_frsky_x.xml b/conf/modules/telemetry_transparent_frsky_x.xml index 5fdaba069a..79c3e5f15c 100644 --- a/conf/modules/telemetry_transparent_frsky_x.xml +++ b/conf/modules/telemetry_transparent_frsky_x.xml @@ -36,7 +36,7 @@ - + diff --git a/conf/modules/telemetry_transparent_gec.xml b/conf/modules/telemetry_transparent_gec.xml index d74581ae61..aa0adecc65 100644 --- a/conf/modules/telemetry_transparent_gec.xml +++ b/conf/modules/telemetry_transparent_gec.xml @@ -16,7 +16,7 @@ - + diff --git a/conf/modules/telemetry_transparent_udp.xml b/conf/modules/telemetry_transparent_udp.xml index c0936f507e..8ccc2375d3 100644 --- a/conf/modules/telemetry_transparent_udp.xml +++ b/conf/modules/telemetry_transparent_udp.xml @@ -21,7 +21,7 @@ - + diff --git a/conf/modules/telemetry_transparent_usb.xml b/conf/modules/telemetry_transparent_usb.xml index 3b8eca0088..57ebf8cc19 100644 --- a/conf/modules/telemetry_transparent_usb.xml +++ b/conf/modules/telemetry_transparent_usb.xml @@ -17,7 +17,7 @@ - + diff --git a/conf/modules/telemetry_w5100.xml b/conf/modules/telemetry_w5100.xml index d98acbf0ab..2260f069c3 100644 --- a/conf/modules/telemetry_w5100.xml +++ b/conf/modules/telemetry_w5100.xml @@ -26,7 +26,7 @@ - + diff --git a/conf/modules/telemetry_xbee_api.xml b/conf/modules/telemetry_xbee_api.xml index 57a02f8bfb..7647e7809a 100644 --- a/conf/modules/telemetry_xbee_api.xml +++ b/conf/modules/telemetry_xbee_api.xml @@ -20,7 +20,7 @@ - + diff --git a/conf/modules/traffic_info.xml b/conf/modules/traffic_info.xml index e3ab945471..a639323218 100644 --- a/conf/modules/traffic_info.xml +++ b/conf/modules/traffic_info.xml @@ -9,11 +9,11 @@ - + - - - + + + diff --git a/conf/userconf/OPENUAS/openuas_all_ac.xml b/conf/userconf/OPENUAS/openuas_all_ac.xml index ed4318e505..1562e9471a 100644 --- a/conf/userconf/OPENUAS/openuas_all_ac.xml +++ b/conf/userconf/OPENUAS/openuas_all_ac.xml @@ -308,15 +308,4 @@ settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/nav_smooth.xml modules/nav_survey_poly_osam.xml modules/stabilization_attitude_fw.xml" gui_color="#ffffffffffff" /> - diff --git a/conf/userconf/OPENUAS/openuas_obc2014_conf.xml b/conf/userconf/OPENUAS/openuas_obc2014_conf.xml deleted file mode 100644 index 094bfb0344..0000000000 --- a/conf/userconf/OPENUAS/openuas_obc2014_conf.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - diff --git a/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.c b/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.c index f39245f6fb..fb2ab5f6cd 100644 --- a/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.c +++ b/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.c @@ -29,6 +29,7 @@ #include "modules/radio_control/radio_control.h" #include "modules/radio_control/spektrum_arch.h" #include "modules/radio_control/spektrum.h" +#include "modules/core/abi.h" #include "std.h" #include @@ -40,17 +41,18 @@ static bool spektrum_available; -void radio_control_impl_init(void) +void spektrum_init(void) { spektrum_available = false; } -void spektrum_event(void (*frame_handler)(void)) + +void spektrum_event(void) { if (spektrum_available) { radio_control.frame_cpt++; radio_control.time_since_last_frame = 0; radio_control.status = RC_OK; - (*frame_handler)(); + AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_SPEKTRUM_ID, &radio_control); } spektrum_available = false; } diff --git a/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.h b/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.h index a85958ce0a..7c504783e2 100644 --- a/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.h +++ b/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.h @@ -25,8 +25,6 @@ #include "modules/radio_control/spektrum_radio.h" -#define RadioControlEventImp spektrum_event - #if USE_NPS extern void radio_control_feed(void); #endif diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c index 5273c4db45..2899b33cfc 100644 --- a/sw/airborne/arch/sim/sim_ap.c +++ b/sw/airborne/arch/sim/sim_ap.c @@ -10,7 +10,7 @@ #include #include #include "std.h" -#include "modules/intermcu/inter_mcu.h" +#include "main_ap.h" #include "autopilot.h" #include "modules/gps/gps.h" #include "generated/settings.h" @@ -18,7 +18,6 @@ #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "modules/core/commands.h" -#include "firmwares/fixedwing/main_ap.h" #include "modules/datalink/datalink.h" #include "modules/datalink/telemetry.h" #include "generated/flight_plan.h" @@ -64,9 +63,8 @@ value sim_sys_time_task(value unit) value sim_periodic_task(value unit) { - handle_periodic_tasks_ap(); - event_task_ap(); - event_task_fbw(); + main_ap_periodic(); + main_ap_event(); return unit; } @@ -80,8 +78,8 @@ float ftimeofday(void) value sim_init(value unit) { - init_fbw(); - init_ap(); + modules_mcu_init(); + main_ap_init(); return unit; } diff --git a/sw/airborne/autopilot.c b/sw/airborne/autopilot.c index 96193243b0..b6a63625b5 100644 --- a/sw/airborne/autopilot.c +++ b/sw/airborne/autopilot.c @@ -36,11 +36,9 @@ #include "mcu_periph/uart.h" #include "mcu_periph/sys_time.h" #include "mcu_periph/gpio.h" -#include "modules/radio_control/radio_control.h" #include "modules/core/commands.h" -#include "modules/actuators/actuators.h" -//#include "modules/energy/electrical.h" #include "modules/datalink/telemetry.h" +#include "modules/core/abi.h" #include "modules/core/settings.h" #include "generated/settings.h" @@ -49,6 +47,18 @@ struct pprz_autopilot autopilot; +#ifndef AUTOPILOT_RC_ID +#define AUTOPILOT_RC_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(AUTOPILOT_RC_ID) +static abi_event rc_ev; + +static void rc_cb(uint8_t __attribute__((unused)) sender_id, + struct RadioControl __attribute__((unused)) *rc) +{ + // TODO pass the RC struct to the on_rc_frame function + autopilot_on_rc_frame(); +} static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev) { @@ -73,20 +83,6 @@ static void send_dl_value(struct transport_tx *trans, struct link_device *dev) PeriodicSendDlValue(trans, dev); } -#ifdef RADIO_CONTROL -static void send_rc(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values); -} -#endif - -#ifdef ACTUATORS -static void send_actuators(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators); -} -#endif - static void send_minimal_com(struct transport_tx *trans, struct link_device *dev) { float lat = DegOfRad(stateGetPositionLla_f()->lat); @@ -142,17 +138,14 @@ void autopilot_init(void) autopilot_static_init(); #endif + // bind ABI messages + AbiBindMsgRADIO_CONTROL(AUTOPILOT_RC_ID, &rc_ev, rc_cb); + // register messages register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ATTITUDE, send_attitude); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DL_VALUE, send_dl_value); -#ifdef ACTUATORS - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators); -#endif -#ifdef RADIO_CONTROL - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc); -#endif register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_MINIMAL_COM, send_minimal_com); } @@ -160,6 +153,23 @@ void autopilot_init(void) */ void autopilot_periodic(void) { + // first check for failsafe case + autopilot_failsafe_checks(); + + // check if in flight + autopilot_check_in_flight(autopilot_get_motors_on()); + +#if FIXEDWING_FIRMWARE + if (autopilot.flight_time) { +#else + if (autopilot_in_flight()) { +#endif + // flight time is incremented every second + // after takeoff for fixedwing + // when in flight for other firmwares + RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++); + } + #if USE_GENERATED_AUTOPILOT autopilot_generated_periodic(); #else @@ -182,6 +192,10 @@ void autopilot_on_rc_frame(void) #endif } +/** Failsafe checks + */ +void WEAK autopilot_failsafe_checks(void) {} + /** set autopilot mode */ bool autopilot_set_mode(uint8_t new_autopilot_mode) diff --git a/sw/airborne/autopilot.h b/sw/airborne/autopilot.h index fee692be02..eeed6cc875 100644 --- a/sw/airborne/autopilot.h +++ b/sw/airborne/autopilot.h @@ -96,6 +96,10 @@ extern void autopilot_event(void); */ extern void autopilot_on_rc_frame(void); +/** Autopilot periodic failsafe checks + */ +extern void autopilot_failsafe_checks(void); + /** Set new autopilot mode * * @param[in] new_autopilot_mode new mode to set diff --git a/sw/airborne/firmwares/fixedwing/autopilot_firmware.c b/sw/airborne/firmwares/fixedwing/autopilot_firmware.c index e549e1266f..20b1f9a978 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot_firmware.c +++ b/sw/airborne/firmwares/fixedwing/autopilot_firmware.c @@ -31,12 +31,11 @@ #include "state.h" #include "firmwares/fixedwing/nav.h" #include +#include "modules/energy/electrical.h" -// ap copy of fbw readings -struct Electrical ap_electrical; +PRINT_CONFIG_VAR(CONTROL_FREQUENCY) uint8_t lateral_mode; -uint8_t mcu1_status; #if PERIODIC_TELEMETRY #include "modules/datalink/telemetry.h" @@ -56,6 +55,23 @@ uint8_t rc_settings_mode = 0; static void send_mode(struct transport_tx *trans, struct link_device *dev) { + /** Old FBW status + * - bit 0: RC OK + * - bit 1: RC REALLY LOST + * - bit 2: FBW AUTO + * - bit 3: FBW FAILSAFE + * - bit 4: RC averaged channels sent + */ + uint8_t mcu1_status = 0; +#ifdef RADIO_CONTROL + mcu1_status |= (radio_control.status == RC_OK ? (1<<0) : 0); + mcu1_status |= (radio_control.status == RC_REALLY_LOST ? (1<<2) : 0); + mcu1_status |= (radio_control.status == RC_OK ? (1<<4) : 0); +#endif +#ifdef AP_MODE_MANUAL + mcu1_status |= (autopilot_get_mode() == AP_MODE_MANUAL ? 0 : (1<<2)); +#endif + pprz_msg_send_PPRZ_MODE(trans, dev, AC_ID, &autopilot.mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status); } @@ -69,9 +85,9 @@ static void send_estimator(struct transport_tx *trans, struct link_device *dev) static void send_energy(struct transport_tx *trans, struct link_device *dev) { uint8_t throttle = 100 * autopilot.throttle / MAX_PPRZ; - float power = ap_electrical.vsupply * ap_electrical.current; + float power = electrical.vsupply * electrical.current; pprz_msg_send_ENERGY(trans, dev, AC_ID, - &throttle, &ap_electrical.vsupply, &ap_electrical.current, &power, &ap_electrical.charge, &ap_electrical.energy); + &throttle, &electrical.vsupply, &electrical.current, &power, &electrical.charge, &electrical.energy); } // FIXME not the best place @@ -101,6 +117,28 @@ static void send_airspeed(struct transport_tx *trans __attribute__((unused)), pprz_msg_send_AIRSPEED(trans, dev, AC_ID, &airspeed, &zero, &zero, &zero); #endif } + +#if !INTERMCU_AP +#include "modules/radio_control/radio_control.h" +static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) +{ + uint8_t fbw_mode = 1; // old FBW_AUTO +#ifdef AP_MODE_MANUAL + if (autopilot_get_mode() == AP_MODE_MANUAL) { fbw_mode = 0; } // FBW Manual +#endif +#ifdef RADIO_CONTROL + uint8_t rc_status = radio_control.status; + uint8_t rc_rate = radio_control.frame_rate; +#else + uint8_t rc_status = 0; + uint8_t rc_rate = 0; +#endif + pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, + &rc_status, &rc_rate, &fbw_mode, + &electrical.vsupply, &electrical.current); +} +#endif + #endif /* PERIODIC_TELEMETRY */ void autopilot_send_mode(void) @@ -113,14 +151,6 @@ void autopilot_send_mode(void) void autopilot_firmware_init(void) { - ap_electrical.vsupply = 0.f; - ap_electrical.current = 0.f; - ap_electrical.charge = 0.f; - ap_electrical.energy = 0.f; - - ap_electrical.bat_critical = false; - ap_electrical.bat_low = false; - #if PERIODIC_TELEMETRY /* register some periodic message */ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_PPRZ_MODE, send_mode); @@ -131,6 +161,9 @@ void autopilot_firmware_init(void) #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC_SETTINGS, send_rc_settings); #endif +#if !INTERMCU_AP + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_fbw_status); +#endif #endif } @@ -160,12 +193,8 @@ void autopilot_firmware_init(void) /** 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) { + if (electrical.vsupply < CATASTROPHIC_BAT_LEVEL) { t++; } else { t = 0; diff --git a/sw/airborne/firmwares/fixedwing/autopilot_firmware.h b/sw/airborne/firmwares/fixedwing/autopilot_firmware.h index 9911a8f123..5775379deb 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot_firmware.h +++ b/sw/airborne/firmwares/fixedwing/autopilot_firmware.h @@ -40,13 +40,6 @@ #define LATERAL_MODE_NB 4 extern uint8_t lateral_mode; -// ap copy of fbw readings -extern struct Electrical ap_electrical; - -/** Second MCU status (FBW part) - */ -extern uint8_t mcu1_status; - /** Init function */ extern void autopilot_firmware_init(void); diff --git a/sw/airborne/firmwares/fixedwing/autopilot_generated.c b/sw/airborne/firmwares/fixedwing/autopilot_generated.c index 205fab1e70..e392f2225a 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot_generated.c +++ b/sw/airborne/firmwares/fixedwing/autopilot_generated.c @@ -88,27 +88,13 @@ void autopilot_generated_set_motors_on(bool motors_on) autopilot.motors_on = motors_on; } -static inline void copy_from_to_fbw(void) -{ - PPRZ_MUTEX_LOCK(fbw_state_mtx); - PPRZ_MUTEX_LOCK(ap_state_mtx); -#ifdef SetAutoCommandsFromRC - SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels); -#elif defined RADIO_YAW && defined COMMAND_YAW - ap_state->commands[COMMAND_YAW] = fbw_state->channels[RADIO_YAW]; -#endif - PPRZ_MUTEX_UNLOCK(ap_state_mtx); - PPRZ_MUTEX_UNLOCK(fbw_state_mtx); -} void autopilot_generated_on_rc_frame(void) { - - // update electrical from FBW - imcu_get_electrical(&ap_electrical); - - // FIXME what to do here ? - copy_from_to_fbw(); - +#ifdef SetAutoCommandsFromRC + SetAutoCommandsFromRC(commands, radio_control.values); +#elif defined RADIO_YAW && defined COMMAND_YAW + command_set(COMMAND_YAW, radio_control_get(RADIO_YAW)); +#endif } diff --git a/sw/airborne/firmwares/fixedwing/autopilot_static.c b/sw/airborne/firmwares/fixedwing/autopilot_static.c index d81467b04f..b68e9ff640 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot_static.c +++ b/sw/airborne/firmwares/fixedwing/autopilot_static.c @@ -29,8 +29,6 @@ #include "autopilot.h" #include "firmwares/fixedwing/autopilot_static.h" -#include "modules/intermcu/inter_mcu.h" -#include "modules/intermcu/link_mcu.h" #include "state.h" #include "firmwares/fixedwing/nav.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" @@ -42,17 +40,10 @@ #if USE_GPS #include "modules/gps/gps.h" #endif -static bool gps_lost; #include "modules/datalink/downlink.h" -#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1 -static uint8_t mcu1_ppm_cpt; -#endif - static inline uint8_t pprz_mode_update(void); -static inline uint8_t mcu1_status_update(void); -static inline void copy_from_to_fbw(void); /** mode to enter when RC is lost in AP_MODE_MANUAL or AP_MODE_AUTO1 */ #ifndef RC_LOST_MODE @@ -90,9 +81,7 @@ void autopilot_event(void) void autopilot_static_init(void) { autopilot.mode = AP_MODE_AUTO2; - lateral_mode = LATERAL_MODE_MANUAL; - gps_lost = false; ///@todo: properly implement/fix a triggered attitude loop #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP @@ -110,83 +99,40 @@ void autopilot_static_periodic(void) */ void autopilot_static_on_rc_frame(void) { - uint8_t mode_changed = false; - copy_from_to_fbw(); - - /* rc_lost_while_in_use is true if we lost RC in MANUAL or AUTO1 */ - uint8_t rc_lost_while_in_use = bit_is_set(imcu_get_status(), STATUS_RADIO_REALLY_LOST) && - (autopilot_get_mode() == AP_MODE_AUTO1 || autopilot_get_mode() == AP_MODE_MANUAL); - - /* RC_LOST_MODE defaults to AP_MODE_HOME, but it can also be set to NAV_MODE_NAV or MANUAL when the RC receiver is well configured to send failsafe commands */ - if (rc_lost_while_in_use) { // Always: no exceptions! - mode_changed = autopilot_set_mode(RC_LOST_MODE); - } - -#ifdef RADIO_KILL_SWITCH - if (imcu_get_radio(RADIO_KILL_SWITCH) < MIN_PPRZ / 2) { - autopilot_set_kill_throttle(true); - } + // Send back uncontrolled channels. +#ifdef SetAutoCommandsFromRC + SetAutoCommandsFromRC(commands, radio_control.values); +#elif defined RADIO_YAW && defined COMMAND_YAW + command_set(COMMAND_YAW, radio_control_get(RADIO_YAW)); #endif - /* If in-flight, with good GPS but too far, then activate HOME mode - * In MANUAL with good RC, FBW will allow to override. */ - if (autopilot_get_mode() != AP_MODE_HOME && autopilot_get_mode() != AP_MODE_GPS_OUT_OF_ORDER && autopilot.launch) { - if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) { - mode_changed = autopilot_set_mode(AP_MODE_HOME); - } - } - if (bit_is_set(imcu_get_status(), AVERAGED_CHANNELS_SENT)) { - bool pprz_mode_changed = pprz_mode_update(); - mode_changed |= pprz_mode_changed; -#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS - PPRZ_MUTEX_LOCK(fbw_state_mtx); - bool calib_mode_changed = RcSettingsModeUpdate(fbw_state->channels); - PPRZ_MUTEX_UNLOCK(fbw_state_mtx); - rc_settings(calib_mode_changed || pprz_mode_changed); - mode_changed |= calib_mode_changed; -#endif - } - mode_changed |= mcu1_status_update(); - if (mode_changed) { autopilot_send_mode(); } - #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1 /** In AUTO1 mode, compute roll setpoint and pitch setpoint from * \a RADIO_ROLL and \a RADIO_PITCH \n */ if (autopilot_get_mode() == AP_MODE_AUTO1) { /** Roll is bounded between [-AUTO1_MAX_ROLL;AUTO1_MAX_ROLL] */ - h_ctl_roll_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_ROLL), 0., AUTO1_MAX_ROLL); + h_ctl_roll_setpoint = FLOAT_OF_PPRZ(radio_control_get(RADIO_ROLL), 0., AUTO1_MAX_ROLL); /** Pitch is bounded between [-AUTO1_MAX_PITCH;AUTO1_MAX_PITCH] */ - h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_PITCH), 0., AUTO1_MAX_PITCH); + h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(radio_control_get(RADIO_PITCH), 0., AUTO1_MAX_PITCH); #if H_CTL_YAW_LOOP && defined RADIO_YAW /** Yaw is bounded between [-AUTO1_MAX_YAW_RATE;AUTO1_MAX_YAW_RATE] */ - h_ctl_yaw_rate_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_YAW), 0., AUTO1_MAX_YAW_RATE); + h_ctl_yaw_rate_setpoint = FLOAT_OF_PPRZ(radio_control_get(RADIO_YAW), 0., AUTO1_MAX_YAW_RATE); #endif - } /** Else asynchronously set by \a h_ctl_course_loop() */ + // Note that old SetApOnlyCommands is no longer needed without a separated FBW + } else if (autopilot_get_mode() == AP_MODE_MANUAL) { + // Set commands from RC in MANUAL mode + SetCommandsFromRC(commands, radio_control.values); + } + /** Else asynchronously set by \a h_ctl_course_loop() */ /** In AUTO1, throttle comes from RADIO_THROTTLE In MANUAL, the value is copied to get it in the telemetry */ if (autopilot_get_mode() == AP_MODE_MANUAL || autopilot_get_mode() == AP_MODE_AUTO1) { - v_ctl_throttle_setpoint = imcu_get_radio(RADIO_THROTTLE); + v_ctl_throttle_setpoint = radio_control_get(RADIO_THROTTLE); } /** else asynchronously set by v_ctl_climb_loop(); */ - - mcu1_ppm_cpt = imcu_get_ppm_cpt(); -#endif // RADIO_CONTROL - - // update electrical from FBW - imcu_get_electrical(&ap_electrical); - -#ifdef RADIO_CONTROL - /* the SITL check is a hack to prevent "automatic" launch in NPS */ -#ifndef SITL - if (!autopilot.flight_time) { - if (autopilot_get_mode() == AP_MODE_AUTO2 && imcu_get_radio(RADIO_THROTTLE) > THROTTLE_THRESHOLD_TAKEOFF) { - autopilot.launch = true; - } - } -#endif #endif } @@ -210,37 +156,11 @@ void autopilot_static_set_motors_on(bool motors_on) autopilot.motors_on = motors_on; } -#ifdef FAILSAFE_DELAY_WITHOUT_GPS -#define GpsTimeoutError (sys_time.nb_sec - gps.last_3dfix_time > FAILSAFE_DELAY_WITHOUT_GPS) -#endif - /** * Compute desired_course */ void navigation_task(void) { -#if defined FAILSAFE_DELAY_WITHOUT_GPS - /** This section is used for the failsafe of GPS */ - static uint8_t last_pprz_mode; - - /** If aircraft is launched and is in autonomus mode, go into - AP_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */ - if (autopilot.launch) { - if (GpsTimeoutError) { - if (autopilot_get_mode() == AP_MODE_AUTO2 || autopilot_get_mode() == AP_MODE_HOME) { - last_pprz_mode = autopilot_get_mode(); - autopilot_set_mode(AP_MODE_GPS_OUT_OF_ORDER); - autopilot_send_mode(); - gps_lost = true; - } - } else if (gps_lost) { /* GPS is ok */ - /** If aircraft was in failsafe mode, come back in previous mode */ - autopilot_set_mode(last_pprz_mode); - gps_lost = false; - autopilot_send_mode(); - } - } -#endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */ common_nav_periodic_task(); if (autopilot_get_mode() == AP_MODE_HOME) { @@ -266,8 +186,9 @@ void navigation_task(void) v_ctl_altitude_loop(); } - if (autopilot_get_mode() == AP_MODE_AUTO2 || autopilot_get_mode() == AP_MODE_HOME - || autopilot_get_mode() == AP_MODE_GPS_OUT_OF_ORDER) { + if (autopilot_get_mode() == AP_MODE_AUTO2 || + autopilot_get_mode() == AP_MODE_HOME || + autopilot_get_mode() == AP_MODE_GPS_OUT_OF_ORDER) { #ifdef H_CTL_RATE_LOOP /* Be sure to be in attitude mode, not roll */ h_ctl_auto1_rate = false; @@ -279,10 +200,110 @@ void navigation_task(void) } } +/** Failsafe checks + * + * Checks order: + * - mode from RC + * - RC lost (if MANUAL or AUTO1) + * - RC kill switch (any case) + * - launch detect from RC + * - GPS lost (if AUTO2 or HOME) + * - too far from HOME (if not HOME or not GPS_OUT_OF_ORDER) + * + * send mode if changed at the end + */ +void autopilot_failsafe_checks(void) +{ + uint8_t mode_changed = false; + +#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1 + /* check normal mode from RC channel(s) + */ + if (!RadioControlIsLost()) { + bool pprz_mode_changed = pprz_mode_update(); + mode_changed |= pprz_mode_changed; +#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS + bool calib_mode_changed = RcSettingsModeUpdate(radio_control.values); + rc_settings(calib_mode_changed || pprz_mode_changed); + mode_changed |= calib_mode_changed; +#endif + } + + /* RC lost while in use is true if we lost RC in MANUAL or AUTO1 + * + * RC_LOST_MODE defaults to AP_MODE_HOME, but it can also be set to NAV_MODE_NAV or MANUAL when the RC receiver is well configured to send failsafe commands + */ + if (RadioControlIsLost() && + (autopilot_get_mode() == AP_MODE_AUTO1 || + autopilot_get_mode() == AP_MODE_MANUAL)) { + mode_changed |= autopilot_set_mode(RC_LOST_MODE); + } + + /* Check RC kill switch + */ +#ifdef RADIO_KILL_SWITCH + if (radio_control_get(RADIO_KILL_SWITCH) < MIN_PPRZ / 2) { + autopilot_set_kill_throttle(true); // not a mode change, just set kill_throttle + } +#endif + + /* the SITL check is a hack to prevent "automatic" launch in NPS + */ +#ifndef SITL + if (!autopilot.flight_time) { + if (autopilot_get_mode() == AP_MODE_AUTO2 && radio_control_get(RADIO_THROTTLE) > THROTTLE_THRESHOLD_TAKEOFF) { + autopilot.launch = true; // set launch to true from RC throttel up + } + } +#endif + +#endif // RADIO_CONTROL + +#if USE_GPS && (defined FAILSAFE_DELAY_WITHOUT_GPS) + /* This section is used for the failsafe of GPS + */ + static uint8_t last_pprz_mode; + static bool gps_lost = false; + + /** If aircraft is launched and is in autonomus mode, go into + AP_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */ + if (autopilot.launch) { + // check GPS timeout + if (sys_time.nb_sec - gps.last_3dfix_time > FAILSAFE_DELAY_WITHOUT_GPS) { + if (autopilot_get_mode() == AP_MODE_AUTO2 || + autopilot_get_mode() == AP_MODE_HOME) { + last_pprz_mode = autopilot_get_mode(); + mode_changed |= autopilot_set_mode(AP_MODE_GPS_OUT_OF_ORDER); + gps_lost = true; + } + } else if (gps_lost) { /* GPS is ok */ + /** If aircraft was in failsafe mode, come back in previous mode */ + mode_changed |= autopilot_set_mode(last_pprz_mode); + gps_lost = false; + } + } +#endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */ + + /* If in-flight, with good GPS but too far, then activate HOME mode + * In MANUAL with good RC, FBW will allow to override. */ + if (autopilot_get_mode() != AP_MODE_HOME && + autopilot_get_mode() != AP_MODE_GPS_OUT_OF_ORDER && + autopilot.launch) { + if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) { + mode_changed |= autopilot_set_mode(AP_MODE_HOME); + } + } + + // send new mode if needed + if (mode_changed) { + autopilot_send_mode(); + } +} + void attitude_loop(void) { - + // Call vertical climb loop if mode is at least AUTO2 if (autopilot_get_mode() >= AP_MODE_AUTO2) { #if CTRL_VERTICAL_LANDING if (v_ctl_mode == V_CTL_MODE_LANDING) { @@ -295,10 +316,10 @@ void attitude_loop(void) } else { if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) { v_ctl_climb_loop(); - } /* v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB */ - } /* v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE */ + } + } #if CTRL_VERTICAL_LANDING - } /* v_ctl_mode == V_CTL_MODE_LANDING */ + } #endif #if defined V_CTL_THROTTLE_IDLE @@ -306,8 +327,8 @@ void attitude_loop(void) #endif #ifdef V_CTL_POWER_CTL_BAT_NOMINAL - if (ap_electrical.vsupply > 0.) { - v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / ap_electrical.vsupply; + if (electrical.vsupply > 0.f) { + v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / electrical.vsupply; v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint); } #endif @@ -320,28 +341,21 @@ void attitude_loop(void) } } - h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */ - v_ctl_throttle_slew(); - PPRZ_MUTEX_LOCK(ap_state_mtx); - AP_COMMAND_SET_THROTTLE(v_ctl_throttle_slewed); - AP_COMMAND_SET_ROLL(-h_ctl_aileron_setpoint); - AP_COMMAND_SET_PITCH(h_ctl_elevator_setpoint); - AP_COMMAND_SET_YAW(h_ctl_rudder_setpoint); - AP_COMMAND_SET_CL(h_ctl_flaps_setpoint); - PPRZ_MUTEX_UNLOCK(ap_state_mtx); - -#if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK - link_mcu_send(); -#elif defined INTER_MCU && defined SINGLE_MCU - /**Directly set the flag indicating to FBW that shared buffer is available*/ - inter_mcu_received_ap = true; -#endif + // Call attitude control and set commands if mode is at least AUTO1 + if (autopilot_get_mode() >= AP_MODE_AUTO1) { + h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */ + v_ctl_throttle_slew(); + AP_COMMAND_SET_THROTTLE(v_ctl_throttle_slewed); + AP_COMMAND_SET_ROLL(-h_ctl_aileron_setpoint); + AP_COMMAND_SET_PITCH(h_ctl_elevator_setpoint); + AP_COMMAND_SET_YAW(h_ctl_rudder_setpoint); + AP_COMMAND_SET_CL(h_ctl_flaps_setpoint); + } } -/******************** Interaction with FBW *****************************/ -/** Update paparazzi mode. +/** Update paparazzi mode from RC */ #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1 static inline uint8_t pprz_mode_update(void) @@ -353,22 +367,28 @@ static inline uint8_t pprz_mode_update(void) #endif ) { #ifndef RADIO_AUTO_MODE - return autopilot_set_mode(AP_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE))); + uint8_t nm = AP_MODE_OF_PULSE(radio_control_get(RADIO_MODE)); + bool b = autopilot_set_mode(nm); + return b; + //return autopilot_set_mode(AP_MODE_OF_PULSE(radio_control_get(RADIO_MODE))); #else INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.") - /* If RADIO_AUTO_MODE is enabled mode swithing will be seperated between two switches/channels - * RADIO_MODE will switch between AP_MODE_MANUAL and any AP_MODE_AUTO mode selected by RADIO_AUTO_MODE. + /* If RADIO_AUTO_MODE is enabled mode swithing will be seperated between + * two switches/channels + * RADIO_MODE will switch between AP_MODE_MANUAL and any AP_MODE_AUTO mode + * selected by RADIO_AUTO_MODE. * - * This is mainly a cludge for entry level radios with no three-way switch but two available two-way switches which can be used. + * This is mainly a cludge for entry level radios with no three-way switch + * but two available two-way switches which can be used. */ - if (AP_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)) == AP_MODE_MANUAL) { + if (AP_MODE_OF_PULSE(radio_control_get(RADIO_MODE)) == AP_MODE_MANUAL) { /* RADIO_MODE in MANUAL position */ return autopilot_set_mode(AP_MODE_MANUAL); } else { /* RADIO_MODE not in MANUAL position. * Select AUTO mode bassed on RADIO_AUTO_MODE channel */ - return autopilot_set_mode((imcu_get_radio(RADIO_AUTO_MODE) > THRESHOLD2) ? AP_MODE_AUTO2 : AP_MODE_AUTO1); + return autopilot_set_mode((radio_control_get(RADIO_AUTO_MODE) > THRESHOLD2) ? AP_MODE_AUTO2 : AP_MODE_AUTO1); } #endif // RADIO_AUTO_MODE } else { @@ -382,29 +402,4 @@ static inline uint8_t pprz_mode_update(void) } #endif -static inline uint8_t mcu1_status_update(void) -{ - uint8_t new_status = imcu_get_status(); - if (mcu1_status != new_status) { - bool changed = ((mcu1_status & MASK_FBW_CHANGED) != (new_status & MASK_FBW_CHANGED)); - mcu1_status = new_status; - return changed; - } - return false; -} - -/** Send back uncontrolled channels. - */ -static inline void copy_from_to_fbw(void) -{ - PPRZ_MUTEX_LOCK(fbw_state_mtx); - PPRZ_MUTEX_LOCK(ap_state_mtx); -#ifdef SetAutoCommandsFromRC - SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels); -#elif defined RADIO_YAW && defined COMMAND_YAW - ap_state->commands[COMMAND_YAW] = fbw_state->channels[RADIO_YAW]; -#endif - PPRZ_MUTEX_UNLOCK(ap_state_mtx); - PPRZ_MUTEX_UNLOCK(fbw_state_mtx); -} diff --git a/sw/airborne/firmwares/fixedwing/autopilot_utils.h b/sw/airborne/firmwares/fixedwing/autopilot_utils.h index 89ae592b01..3d9914585c 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot_utils.h +++ b/sw/airborne/firmwares/fixedwing/autopilot_utils.h @@ -30,7 +30,6 @@ #define AUTOPILOT_UTILS_H #include "std.h" -#include "modules/intermcu/inter_mcu.h" #include "generated/airframe.h" /** Get mode from pulse @@ -61,45 +60,45 @@ */ // COMMAND_ROLL -#define AP_COMMAND_SET_ROLL(_roll) { ap_state->commands[COMMAND_ROLL] = _roll; } +#define AP_COMMAND_SET_ROLL(_roll) { command_set(COMMAND_ROLL, _roll); } // COMMAND_PITCH -#define AP_COMMAND_SET_PITCH(_pitch) { ap_state->commands[COMMAND_PITCH] = _pitch; } +#define AP_COMMAND_SET_PITCH(_pitch) { command_set(COMMAND_PITCH, _pitch); } // COMMAND_YAW #if H_CTL_YAW_LOOP && defined COMMAND_YAW -#define AP_COMMAND_SET_YAW(_yaw) { ap_state->commands[COMMAND_YAW] = _yaw; } +#define AP_COMMAND_SET_YAW(_yaw) { command_set(COMMAND_YAW, _yaw); } #else #define AP_COMMAND_SET_YAW(_yaw) {} #endif // COMMAND_THROTTLE #define AP_COMMAND_SET_THROTTLE(_throttle) { \ - ap_state->commands[COMMAND_THROTTLE] = _throttle; \ + command_set(COMMAND_THROTTLE, _throttle); \ autopilot.throttle = _throttle; \ } // COMMAND_CL #if H_CTL_CL_LOOP && defined COMMAND_CL -#define AP_COMMAND_SET_CL(_cl) { ap_state->commands[COMMAND_CL] = cl; } +#define AP_COMMAND_SET_CL(_cl) { command_set(COMMAND_CL, _cl); } #else #define AP_COMMAND_SET_CL(_cl) {} #endif // ROLL setpoint from RADIO #define AP_SETPOINT_ROLL(_roll, _max) { \ - _roll = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_ROLL], 0., _max); \ + _roll = FLOAT_OF_PPRZ(radio_control_get(RADIO_ROLL), 0., _max); \ } // PITCH setpoint from RADIO #define AP_SETPOINT_PITCH(_pitch, _max) { \ - _pitch = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_PITCH], 0., _max); \ + _pitch = FLOAT_OF_PPRZ(radio_control_get(RADIO_PITCH), 0., _max); \ } // PITCH setpoint from RADIO #if H_CTL_YAW_LOOP && defined RADIO_YAW #define AP_SETPOINT_YAW_RATE(_yaw, _max) { \ - _yaw = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_YAW], 0., _max); \ + _yaw = FLOAT_OF_PPRZ(radio_control_get(RADIO_YAW), 0., _max); \ } #else #define AP_SETPOINT_YAW_RATE(_yaw, _max) {} @@ -107,7 +106,7 @@ // THROTTLE setpoint from RADIO #define AP_SETPOINT_THROTTLE(_throttle) { \ - _throttle = fbw_state->channels[RADIO_THROTTLE]; \ + _throttle = radio_control_get(RADIO_THROTTLE); \ } diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c index 2ad247ab91..6c642fed37 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c @@ -289,8 +289,8 @@ void v_ctl_guidance_loop(void) #endif #ifdef V_CTL_POWER_CTL_BAT_NOMINAL - if (ap_electrical.vsupply > 0.) { - v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / ap_electrical.vsupply; + if (electrical.vsupply > 0.) { + v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / electrical.vsupply; v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint); } #endif diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c index 5169104bbe..c0f2ee3eef 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c @@ -206,8 +206,8 @@ void v_ctl_guidance_loop(void) #endif #ifdef V_CTL_POWER_CTL_BAT_NOMINAL - if (ap_electrical.vsupply > 0.) { - v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / ap_electrical.vsupply; + if (electrical.vsupply > 0.) { + v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / electrical.vsupply; v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint); } #endif diff --git a/sw/airborne/firmwares/fixedwing/main.c b/sw/airborne/firmwares/fixedwing/main.c deleted file mode 100644 index a0020aa230..0000000000 --- a/sw/airborne/firmwares/fixedwing/main.c +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Copyright (C) 2005 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file firmwares/fixedwing/main.c - * Main loop used both on single and dual MCU configuration. - */ - - -#ifdef FBW -#include "firmwares/fixedwing/main_fbw.h" -#define Fbw(f) f ## _fbw() -#else -#define Fbw(f) -#endif - -#ifdef AP -#include "firmwares/fixedwing/main_ap.h" -#define Ap(f) f ## _ap() -#else -#define Ap(f) -#endif - -int main(void) -{ - Fbw(init); - Ap(init); - while (1) { - Fbw(handle_periodic_tasks); - Ap(handle_periodic_tasks); - Fbw(event_task); - Ap(event_task); - } - return 0; -} diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c deleted file mode 100644 index cdf7c57132..0000000000 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ /dev/null @@ -1,239 +0,0 @@ -/* - * Copyright (C) 2003-2021 The Paparazzi Team - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file firmwares/fixedwing/main_ap.c - * - * AP ( AutoPilot ) tasks - * - * This process is reponsible for the collecting the different sensors data, - * calling the appropriate estimation algorithms and running the different control loops. - */ - -#define MODULES_C - -#define ABI_C - -#include - -#include "firmwares/fixedwing/main_ap.h" -#include "mcu.h" -#include "mcu_periph/sys_time.h" -#include "modules/intermcu/inter_mcu.h" -#include "modules/intermcu/link_mcu.h" - -#include "generated/airframe.h" -#include "generated/modules.h" -#include "modules/core/abi.h" - -#include "led.h" - -#ifdef USE_NPS -#include "nps_autopilot.h" -#endif - -/* Default trim commands for roll, pitch and yaw */ -#ifndef COMMAND_ROLL_TRIM -#define COMMAND_ROLL_TRIM 0 -#endif - -#ifndef COMMAND_PITCH_TRIM -#define COMMAND_PITCH_TRIM 0 -#endif - -#ifndef COMMAND_YAW_TRIM -#define COMMAND_YAW_TRIM 0 -#endif - -/* if PRINT_CONFIG is defined, print some config options */ -PRINT_CONFIG_VAR(PERIODIC_FREQUENCY) -#if !USE_GENERATED_AUTOPILOT -PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY) -#endif -PRINT_CONFIG_VAR(CONTROL_FREQUENCY) - -/* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h - * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file - */ -#ifndef TELEMETRY_FREQUENCY -#define TELEMETRY_FREQUENCY 60 -#endif -PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY) - - -#if USE_IMU -#ifdef AHRS_PROPAGATE_FREQUENCY -#if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY) -#warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY" -INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY) -#endif -#endif -#endif // USE_IMU - -/** - * IDs for timers - */ -tid_t modules_mcu_core_tid; // single step -tid_t modules_sensors_tid; -//tid_t modules_radio_control_tid; // done in FBW -tid_t modules_gnc_tid; // estimation, control, actuators, default in a single step -tid_t modules_datalink_tid; - -#define SYS_PERIOD (1.f / PERIODIC_FREQUENCY) -#define SENSORS_PERIOD (1.f / PERIODIC_FREQUENCY) -#define DATALINK_PERIOD (1.f / TELEMETRY_FREQUENCY) - -void init_ap(void) -{ -#ifndef SINGLE_MCU - modules_mcu_init(); -#endif - 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(); - - // register timers with temporal dependencies - 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_datalink_tid = sys_time_register_timer(DATALINK_PERIOD, NULL); - - /* set initial trim values. - * these are passed to fbw via inter_mcu. - */ - PPRZ_MUTEX_LOCK(ap_state_mtx); - ap_state->command_roll_trim = COMMAND_ROLL_TRIM; - ap_state->command_pitch_trim = COMMAND_PITCH_TRIM; - ap_state->command_yaw_trim = COMMAND_YAW_TRIM; - PPRZ_MUTEX_UNLOCK(ap_state_mtx); - -#if USE_IMU - // send body_to_imu from here for now - AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); -#endif - -} - - -void handle_periodic_tasks_ap(void) -{ - if (sys_time_check_and_ack_timer(modules_sensors_tid)) { - modules_sensors_periodic_task(); - } - - if (sys_time_check_and_ack_timer(modules_gnc_tid)) { - modules_estimation_periodic_task(); - modules_control_periodic_task(); - modules_default_periodic_task(); - } - - 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(); - modules_datalink_periodic_task(); // FIXME integrate above -#if defined DATALINK || defined SITL - RunOnceEvery(TELEMETRY_FREQUENCY, datalink_time++); -#endif - } - -} - - - -/**************************** Periodic tasks ***********************************/ - -/** - * Send a series of initialisation messages followed by a stream of periodic ones. - */ -void reporting_task(void) -{ - static uint8_t boot = true; - - /* initialisation phase during boot */ - if (boot) { -#if DOWNLINK - autopilot_send_version(); -#endif - boot = false; - } - /* then report periodicly */ - else { -#if PERIODIC_TELEMETRY - periodic_telemetry_send_Ap(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); -#endif - } -} - - -/*********** EVENT ***********************************************************/ -void event_task_ap(void) -{ -#ifndef SINGLE_MCU - /* for SINGLE_MCU done in main_fbw */ - /* event functions for mcu peripherals: i2c, usb_serial.. */ - modules_mcu_event_task(); -#endif /* SINGLE_MCU */ - modules_core_event_task(); - modules_sensors_event_task(); - modules_estimation_event_task(); - modules_control_event_task(); - modules_datalink_event_task(); - modules_default_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(); - } - -} /* event_task_ap() */ - diff --git a/sw/airborne/firmwares/fixedwing/main_ap.h b/sw/airborne/firmwares/fixedwing/main_ap.h deleted file mode 100644 index 08db0fb6bc..0000000000 --- a/sw/airborne/firmwares/fixedwing/main_ap.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * Copyright (C) 2005 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file firmwares/fixedwing/main_ap.h - * - * AP ( AutoPilot ) process API - * - */ - -#ifndef AP_H -#define AP_H - -extern void init_ap(void); -extern void handle_periodic_tasks_ap(void); -extern void event_task_ap(void); - -extern void reporting_task(void); - -#endif diff --git a/sw/airborne/firmwares/fixedwing/main_chibios.h b/sw/airborne/firmwares/fixedwing/main_chibios.h deleted file mode 100644 index 19434ad16a..0000000000 --- a/sw/airborne/firmwares/fixedwing/main_chibios.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * Copyright (C) 2013-2015 Gautier Hattenberger, Alexandre Bustico - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file firmwares/fixedwing/main_chibios.h - */ - -#ifndef MAIN_CHIBIOS_H -#define MAIN_CHIBIOS_H - -#include - -/** Terminate all autopilot threads - * Wait until proper stop - */ -extern void pprz_terminate_autopilot_threads(void); - -#endif /* MAIN_CHIBIOS_H */ diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c deleted file mode 100644 index 9719b663e9..0000000000 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ /dev/null @@ -1,452 +0,0 @@ -/* - * Copyright (C) 2003-2010 The Paparazzi Team - * - * This file is part of Paparazzi. - * - * Paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * Paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** - * @file firmwares/fixedwing/main_fbw.c - * - * FBW ( FlyByWire ) process - * - * This process is responsible for decoding radio control, generating actuators - * signals either from the radio control or from the commands provided by the - * AP (autopilot) process. It also performs a telemetry task and a low level monitoring - * ( for parameters like the supply ) - */ -#include "firmwares/fixedwing/main_fbw.h" - -#include "generated/airframe.h" - -#include "mcu.h" -#include "mcu_periph/sys_time.h" -#include "modules/core/commands.h" -#include "modules/actuators/actuators.h" -#include "modules/energy/electrical.h" -#include "modules/radio_control/radio_control.h" -#include "autopilot.h" -#include "paparazzi.h" -#include "mcu_periph/i2c.h" -#include "mcu_periph/uart.h" - -#ifdef USE_NPS -#include "nps_autopilot.h" -#endif - -#if PERIODIC_TELEMETRY -#include "modules/datalink/telemetry.h" -#endif - -#ifdef FBW_DATALINK -#include "firmwares/fixedwing/fbw_datalink.h" -#endif - -#include "modules/intermcu/inter_mcu.h" -#include "modules/intermcu/link_mcu.h" - -uint8_t fbw_mode; - -/** Trim commands for roll, pitch and yaw. - * These are updated from the trim commands in ap_state via inter_mcu - */ -pprz_t command_roll_trim; -pprz_t command_pitch_trim; -pprz_t command_yaw_trim; - -volatile uint8_t fbw_new_actuators = 0; - -uint8_t ap_has_been_ok = false; - -tid_t fbw_periodic_tid; ///< id for periodic_task_fbw() timer -tid_t electrical_tid; ///< id for electrical_periodic() timer - -/********** RADIO CONTROL DEFINES ************************************************/ -#ifdef RADIO_CONTROL -/** - * Defines behavior when the RC is lost, default goes to AUTO - */ -void radio_lost_mode(void); - -#if !OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP && !OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE -// default case -void radio_lost_mode(void) -{ - fbw_mode = FBW_MODE_AUTO; -} -#endif /* default */ - -#if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP -#warning WARNING DANGER: OUTBACK_CHALLENGE RULE RC_LOST_NO_AP defined. If you loose RC you will NOT go to automatically go to AUTO2 Anymore!! -static inline void set_failsafe_mode(void); -void radio_lost_mode(void) -{ - set_failsafe_mode(); -} -#endif /* OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP */ - -#if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE -#warning WARNING DANGER: OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE defined. If you ever temporarly lost RC while in manual, you will failsafe forever even if RC is restored -void radio_lost_mode(void) -{ - commands[COMMAND_FORCECRASH] = 9600; -} -#endif /* OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE */ - - -static inline void handle_rc_frame(void) -{ - fbw_mode = FBW_MODE_OF_PPRZ(radio_control.values[RADIO_MODE]); - if (fbw_mode == FBW_MODE_MANUAL) { - SetCommandsFromRC(commands, radio_control.values); - fbw_new_actuators = 1; - } -} - -void radio_control_event(void) -{ - RadioControlEvent(handle_rc_frame); -} - -void radio_control_periodic_handle(void) -{ - radio_control_periodic_task(); - if (fbw_mode == FBW_MODE_MANUAL && radio_control.status == RC_REALLY_LOST) { - radio_lost_mode(); - } -} -#else /* no RADIO_CONTROL */ -void radio_control_event(void) {} -void radio_control_periodic_handle(void) {} -#endif /* RADIO_CONTROL */ - -/********** FBW_DATALINK defines ************************************************/ -#ifdef FBW_DATALINK -void fbw_datalink_periodic_handle(void) -{ - fbw_datalink_periodic(); -} -void fbw_datalink_event_handle(void) -{ - fbw_datalink_event(); -} -#else /* no FBW_DATALINK */ -void fbw_datalink_periodic_handle(void) {} -void fbw_datalink_event_handle(void) {} -#endif /* FBW_DATALINK */ - -/********** ACTUATORS defines ************************************************/ -void update_actuators(void); -#if defined ACTUATORS && defined INTER_MCU -void update_actuators(void) -{ - if (fbw_new_actuators > 0) { - pprz_t trimmed_commands[COMMANDS_NB]; - int i; - for (i = 0; i < COMMANDS_NB; i++) {trimmed_commands[i] = commands[i];} - -#ifdef COMMAND_ROLL - trimmed_commands[COMMAND_ROLL] += ClipAbs(command_roll_trim, MAX_PPRZ / 10); -#endif /* COMMAND_ROLL */ - -#ifdef COMMAND_PITCH - trimmed_commands[COMMAND_PITCH] += ClipAbs(command_pitch_trim, MAX_PPRZ / 10); -#endif /* COMMAND_PITCH */ - -#ifdef COMMAND_YAW - trimmed_commands[COMMAND_YAW] += ClipAbs(command_yaw_trim, MAX_PPRZ); -#endif /* COMMAND_YAW */ - - SetActuatorsFromCommands(trimmed_commands, autopilot_get_mode()); - fbw_new_actuators = 0; - } -} -#else -void update_actuators(void) {}; -#endif /* ACTUATORS && INTER_MCU */ - - -/********** INTER_MCU defines ************************************************/ -#ifdef INTER_MCU -// pre-and post functions -void pre_inter_mcu_received_ap(void); -void post_inter_mcu_received_ap(void); - -/** - * FBW_MODE_INTER_MCU_FAILSAFE defines what happens when inter MCU fails - * Defaults goes to AUTO - */ -#define FBW_MODE_INTER_MCU_FAILSAFE FBW_MODE_AUTO - -#if !OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE && !OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE -void pre_inter_mcu_received_ap(void) {}; -void post_inter_mcu_received_ap(void) {}; -#endif /* DEFAULT */ - - -#if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP -#undef FBW_MODE_INTER_MCU_FAILSAFE -#define FBW_MODE_INTER_MCU_FAILSAFE fbw_mode -#endif /* OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP */ - - -#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE -#warning OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE loose ap is forced crash -void pre_inter_mcu_received_ap(void) -{ - if (ap_ok) { - ap_has_been_ok = true; - } - if ((ap_has_been_ok) && (!ap_ok)) { - commands[COMMAND_FORCECRASH] = 9600; - } -} -void post_inter_mcu_received_ap(void) {}; -#endif /* OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE */ - - -#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE -#warning DANGER DANGER DANGER DANGER: Outback Challenge Rule FORCE-CRASH-RULE: DANGER DANGER: -#warning AP is now capable to FORCE your FBW in failsafe mode EVEN IF RC IS NOT LOST: Consider the consequences. -// OUTBACK: JURY REQUEST FLIGHT TERMINATION -void pre_inter_mcu_received_ap(void) {}; -void post_inter_mcu_received_ap(void) -{ - if (commands[COMMAND_FORCECRASH] >= 8000) { - set_failsafe_mode(); - } -} -#endif /* OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE */ - - -/** - * Failsafe function - */ -static inline void set_failsafe_mode(void) -{ - fbw_mode = FBW_MODE_FAILSAFE; - SetCommands(commands_failsafe); - fbw_new_actuators = 1; -} - -/** - * Handles inter_mcu periodic checks - */ -void inter_mcu_periodic_handle(void) -{ - inter_mcu_periodic_task(); - if (fbw_mode == FBW_MODE_AUTO && !ap_ok) { - set_failsafe_mode(); - } - -#if defined MCU_UART_LINK || defined MCU_CAN_LINK - inter_mcu_fill_fbw_state(); - link_mcu_periodic_task(); -#endif /* defined MCU_UART_LINK || defined MCU_CAN_LINK */ -} - -/** - * Handles inter_mcu event - */ -void inter_mcu_event_handle(void) -{ -#if defined MCU_SPI_LINK | defined MCU_UART_LINK - link_mcu_event_task(); -#endif /* MCU_SPI_LINK */ - - pre_inter_mcu_received_ap(); - - if (inter_mcu_received_ap) { - inter_mcu_received_ap = false; - inter_mcu_event_task(); - - PPRZ_MUTEX_LOCK(ap_state_mtx); - command_roll_trim = ap_state->command_roll_trim; - command_pitch_trim = ap_state->command_pitch_trim; - command_yaw_trim = ap_state->command_yaw_trim; - if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) { - fbw_mode = FBW_MODE_INTER_MCU_FAILSAFE; - } - if (fbw_mode == FBW_MODE_AUTO) { - SetCommands(ap_state->commands); - } else { -#if SET_AP_ONLY_COMMANDS - SetApOnlyCommands(ap_state->commands); -#endif /* SET_AP_ONLY_COMMANDS */ - } - fbw_new_actuators = 1; - PPRZ_MUTEX_UNLOCK(ap_state_mtx); - -#ifdef SINGLE_MCU - inter_mcu_fill_fbw_state(); -#endif /* SINGLE_MCU - The buffer is filled even if the last receive was not correct */ - } - - post_inter_mcu_received_ap(); - - update_actuators(); - -#ifdef MCU_SPI_LINK - if (link_mcu_received) { - link_mcu_received = false; - inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */ - link_mcu_restart(); /** Prepares the next SPI communication */ - } -#endif /* MCU_SPI_LINK */ -} -#else /* no INTER_MCU */ -void inter_mcu_periodic_handle(void) {} -void inter_mcu_event_handle(void) {} -#endif /* INTER_MCU */ - -/********** PERIODIC MESSAGES ************************************************/ -#if PERIODIC_TELEMETRY -static void send_commands(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, commands); -} - -#ifdef RADIO_CONTROL -static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, - &(radio_control.status), &(radio_control.frame_rate), &fbw_mode, &electrical.vsupply, &electrical.current); -} - -static void send_rc(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values); -} - -#else /* no RADIO_CONTROL */ -static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) -{ - uint8_t dummy = 0; - pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, - &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current); -} -#endif /* RADIO_CONTROL */ - -#ifdef ACTUATORS -static void send_actuators(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators); -} -#endif /* ACTUATORS */ - -void periodic_telemetry_handle(void) -{ - periodic_telemetry_send_Fbw(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); -} - -#else -void periodic_telemetry_handle(void) {} -#endif /* PERIODIC_TELEMETRY */ - -/********** INIT *************************************************************/ -void init_fbw(void) -{ - mcu_init(); - -#if !(DISABLE_ELECTRICAL) - electrical_init(); -#endif - -#ifdef ACTUATORS - actuators_init(); - /* Load the failsafe defaults */ - SetCommands(commands_failsafe); - fbw_new_actuators = 1; -#endif /* ACTUATORS */ - -#ifdef RADIO_CONTROL - radio_control_init(); -#endif /* RADIO_CONTROL */ - -#ifdef INTER_MCU - inter_mcu_init(); -#endif /* INTER_MCU */ - -#if defined MCU_SPI_LINK || defined MCU_CAN_LINK - link_mcu_init(); -#endif /* MCU_SPI_LINK || MCU_CAN_LINK */ - -#ifdef MCU_SPI_LINK - link_mcu_restart(); -#endif /* MCU_SPI_LINK */ - - fbw_mode = FBW_MODE_FAILSAFE; - - /**** start timers for periodic functions *****/ - fbw_periodic_tid = sys_time_register_timer((1. / 60.), NULL); - electrical_tid = sys_time_register_timer(0.1, NULL); - -#if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_fbw_status); - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands); - -#ifdef ACTUATORS - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators); -#endif /* ACTUATORS */ - -#ifdef RADIO_CONTROL - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc); -#endif /* RADIO_CONTROL */ - -#endif /* PERIODIC_TELEMETRY */ - -} - -/********** EVENT ************************************************************/ - -void event_task_fbw(void) -{ - radio_control_event(); - - /* event functions for mcu peripherals: i2c, usb_serial.. */ - mcu_event(); - - inter_mcu_event_handle(); - - fbw_datalink_event_handle(); -} - -/************* PERIODIC ******************************************************/ -void periodic_task_fbw(void) -{ - fbw_datalink_periodic_handle(); - - radio_control_periodic_handle(); - - inter_mcu_periodic_handle(); - - periodic_telemetry_handle(); -} - -void handle_periodic_tasks_fbw(void) -{ - if (sys_time_check_and_ack_timer(fbw_periodic_tid)) { - periodic_task_fbw(); - } - -#if !(DISABLE_ELECTRICAL) - if (sys_time_check_and_ack_timer(electrical_tid)) { - electrical_periodic(); - } -#endif -} diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.h b/sw/airborne/firmwares/fixedwing/main_fbw.h deleted file mode 100644 index b3b715d176..0000000000 --- a/sw/airborne/firmwares/fixedwing/main_fbw.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - * Copyright (C) 2005 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file firmwares/fixedwing/main_fbw.h - * - * FBW ( FlyByWire ) process API - * - */ -#ifndef FBW_H -#define FBW_H - -#include "std.h" - -/** Fly by wire modes */ -#define FBW_MODE_MANUAL 0 -#define FBW_MODE_AUTO 1 -#define FBW_MODE_FAILSAFE 2 -#define FBW_MODE_OF_PPRZ(mode) ((mode) < THRESHOLD_MANUAL_PPRZ ? FBW_MODE_MANUAL : FBW_MODE_AUTO) - -extern uint8_t fbw_mode; -extern bool failsafe_mode; - -void init_fbw(void); -void handle_periodic_tasks_fbw(void); -void periodic_task_fbw(void); -void event_task_fbw(void); - -void radio_control_event(void); - -void fbw_datalink_periodic_handle(void); -void fbw_datalink_event_handle(void); - -void radio_control_periodic_handle(void); - -void inter_mcu_periodic_handle(void); -void inter_mcu_event_handle(void); - -void periodic_telemetry_handle(void); - -#endif /* FBW_H */ diff --git a/sw/airborne/firmwares/fixedwing/main_recovery.c b/sw/airborne/firmwares/fixedwing/main_recovery.c new file mode 100644 index 0000000000..37e1fd12b3 --- /dev/null +++ b/sw/airborne/firmwares/fixedwing/main_recovery.c @@ -0,0 +1,164 @@ +/* + * Copyright (C) 2022 Gautier Hattenberger + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + * + */ + +/** + * @file firmwares/fixedwing/main_recovery.c + * + * Recovery mode: run manual mode in case of hardfault + * Based on legacy FBW + * + */ + +#include "firmwares/fixedwing/main_recovery.h" +#include "generated/airframe.h" +#include "generated/modules.h" +#include "modules/core/abi.h" +#include "modules/radio_control/radio_control.h" +#include "modules/core/commands.h" +#include "modules/energy/electrical.h" +#include "autopilot_utils.h" +#if PERIODIC_TELEMETRY +#include "modules/datalink/telemetry.h" +#endif + +uint8_t recovery_mode; + +tid_t periodic_tid; ///< id for periodic task timer +tid_t electrical_tid; ///< id for electrical_periodic() timer +#if PERIODIC_TELEMETRY +tid_t telemetry_tid; ///< id for periodic telemetry +#endif + +#ifndef RECOVERY_RC_ID +#define RECOVERY_RC_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(RECOVERY_RC_ID) +static abi_event rc_ev; + +#define RECOVERY_MODE_OF_PPRZ(mode) ((mode) < THRESHOLD_MANUAL_PPRZ ? RECOVERY_MODE_MANUAL : RECOVERY_MODE_FAILSAFE) + +static void rc_cb(uint8_t __attribute__((unused)) sender_id, + struct RadioControl *rc) +{ + recovery_mode = RECOVERY_MODE_OF_PPRZ(rc->values[RADIO_MODE]); + if (recovery_mode == RECOVERY_MODE_MANUAL && rc->status == RC_OK) { + // on RC callback in manual, set commands from RC + SetCommandsFromRC(commands, rc->values); + } +} + +/********** PERIODIC MESSAGES ************************************************/ +#if PERIODIC_TELEMETRY +#include "modules/datalink/telemetry.h" + +static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) +{ +#ifdef RADIO_CONTROL + uint8_t rc_status = radio_control.status; + uint8_t rc_rate = radio_control.frame_rate; +#else + uint8_t rc_status = 0; + uint8_t rc_rate = 0; +#endif + pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, + &rc_status, &rc_rate, &recovery_mode, + &electrical.vsupply, &electrical.current); +} + +#endif /* PERIODIC_TELEMETRY */ + +/********** INIT *************************************************************/ +void main_recovery_init(void) +{ + // mcu init done in main + + // don't call all core modules, only a subset + electrical_init(); + commands_init(); + + modules_radio_control_init(); + modules_actuators_init(); + modules_datalink_init(); + + recovery_mode = RECOVERY_MODE_FAILSAFE; + + /**** start timers for periodic functions *****/ + periodic_tid = sys_time_register_timer((1. / 60.), NULL); + electrical_tid = sys_time_register_timer(0.1, NULL); + + // actuators periodic will update the actuators + // but the autopilot.mode is passed to SetActuatorsFromCommands + // it is set to manual if possible, otherwise behavior is undefined +#ifdef AP_MODE_MANUAL + autopilot.mode = AP_MODE_MANUAL; +#endif + + // Bind to RC event + AbiBindMsgRADIO_CONTROL(RECOVERY_RC_ID, &rc_ev, rc_cb); + +#if PERIODIC_TELEMETRY + telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_fbw_status); +#endif /* PERIODIC_TELEMETRY */ + +} + +/********** EVENT ************************************************************/ + +void main_recovery_event(void) +{ + modules_mcu_event_task(); + modules_radio_control_event_task(); + modules_actuators_event_task(); + modules_datalink_event_task(); +} + +/************* PERIODIC ******************************************************/ + +void main_recovery_periodic(void) +{ + if (sys_time_check_and_ack_timer(periodic_tid)) { + modules_mcu_periodic_task(); + modules_radio_control_periodic_task(); + // if RC is lost or not in manual, set failsafe commands + if (radio_control.status == RC_REALLY_LOST || + recovery_mode == RECOVERY_MODE_FAILSAFE) { + recovery_mode = RECOVERY_MODE_FAILSAFE; + SetCommands(commands_failsafe); + } + modules_actuators_periodic_task(); + } + +#if !(RECOVERY_DISABLE_ELECTRICAL) + if (sys_time_check_and_ack_timer(electrical_tid)) { + electrical_periodic(); + } +#endif + +#if PERIODIC_TELEMETRY + if (sys_time_check_and_ack_timer(telemetry_tid)) { + // only fbw part during recovery + periodic_telemetry_send_Fbw(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); + } +#endif + +} + diff --git a/sw/airborne/firmwares/fixedwing/main_recovery.h b/sw/airborne/firmwares/fixedwing/main_recovery.h new file mode 100644 index 0000000000..880a98f0f1 --- /dev/null +++ b/sw/airborne/firmwares/fixedwing/main_recovery.h @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2022 Gautier Hattenberger + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + * + */ + +/** + * @file firmwares/fixedwing/main_recovery.h + * + * Recovery mode: run manual mode in case of hardfault + * Based on legacy FBW + * + */ +#ifndef MAIN_RECOVERY_H +#define MAIN_RECOVERY_H + +#include "std.h" + +/** recovery modes */ +#define RECOVERY_MODE_MANUAL 0 +#define RECOVERY_MODE_FAILSAFE 2 // for compatibility will old FBW modes + +extern uint8_t recovery_mode; + +void main_recovery_init(void); +void main_recovery_periodic(void); +void main_recovery_event(void); + +#endif /* MAIN_RECOVERY_H */ diff --git a/sw/airborne/firmwares/fixedwing/nav.c b/sw/airborne/firmwares/fixedwing/nav.c index fac75f6146..5dc07cc33d 100644 --- a/sw/airborne/firmwares/fixedwing/nav.c +++ b/sw/airborne/firmwares/fixedwing/nav.c @@ -34,11 +34,13 @@ static unit_t unit __attribute__((unused)); #include "firmwares/fixedwing/nav.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "autopilot.h" -#include "modules/intermcu/inter_mcu.h" #include "modules/gps/gps.h" #include "generated/flight_plan.h" +#if !USE_GENERATED_AUTOPILOT +PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY) +#endif enum oval_status oval_status; @@ -172,20 +174,20 @@ void nav_glide(uint8_t start_wp, uint8_t wp) #define Goto3D(radius) { \ if (autopilot_get_mode() == AP_MODE_AUTO2) { \ - int16_t yaw = imcu_get_radio(RADIO_YAW); \ + int16_t yaw = radio_control_get(RADIO_YAW); \ if (yaw > MIN_DX || yaw < -MIN_DX) { \ carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.); \ carrot_x = Min(carrot_x, MAX_DIST_CARROT); \ carrot_x = Max(carrot_x, -MAX_DIST_CARROT); \ } \ - int16_t pitch = imcu_get_radio(RADIO_PITCH); \ + int16_t pitch = radio_control_get(RADIO_PITCH); \ if (pitch > MIN_DX || pitch < -MIN_DX) { \ carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); \ carrot_y = Min(carrot_y, MAX_DIST_CARROT); \ carrot_y = Max(carrot_y, -MAX_DIST_CARROT); \ } \ v_ctl_mode = V_CTL_MODE_AUTO_ALT; \ - int16_t roll = imcu_get_radio(RADIO_ROLL); \ + int16_t roll = radio_control_get(RADIO_ROLL); \ if (roll > MIN_DX || roll < -MIN_DX) { \ nav_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); \ nav_altitude = Max(nav_altitude, MIN_HEIGHT_CARROT+ground_alt); \ diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h index 4def14f005..436084bf3e 100644 --- a/sw/airborne/firmwares/fixedwing/nav.h +++ b/sw/airborne/firmwares/fixedwing/nav.h @@ -51,8 +51,8 @@ #define Square(_x) ((_x)*(_x)) #define DistanceSquare(p1_x, p1_y, p2_x, p2_y) (Square(p1_x-p2_x)+Square(p1_y-p2_y)) -#define PowerVoltage() (ap_electrical.vsupply) -#define RcRoll(travel) (imcu_get_radio(RADIO_ROLL) * (float)travel /(float)MAX_PPRZ) +#define PowerVoltage() (electrical.vsupply) +#define RcRoll(travel) (radio_control_get(RADIO_ROLL) * (float)travel /(float)MAX_PPRZ) enum oval_status { OR12, OC2, OR21, OC1 }; diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h b/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h index 95e6571245..6b6ff3dcea 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h @@ -30,6 +30,7 @@ #include "generated/airframe.h" #include "modules/radio_control/radio_control.h" +#include "generated/modules.h" #define AUTOPILOT_THROTTLE_THRESHOLD (MAX_PPRZ / 20) #define AUTOPILOT_YAW_THRESHOLD (MAX_PPRZ * 19 / 20) diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_static.c b/sw/airborne/firmwares/rotorcraft/autopilot_static.c index 4d4dbb4f19..72e1ff27f3 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_static.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot_static.c @@ -391,3 +391,50 @@ void autopilot_static_on_rc_frame(void) } } + +/** mode to enter when RC is lost while using a mode with RC input (not AP_MODE_NAV) */ +#ifndef RC_LOST_MODE +#define RC_LOST_MODE AP_MODE_FAILSAFE +#endif + +void autopilot_failsafe_checks(void) +{ + if (radio_control.status == RC_REALLY_LOST && + autopilot_get_mode() != AP_MODE_KILL && + autopilot_get_mode() != AP_MODE_HOME && + autopilot_get_mode() != AP_MODE_FAILSAFE && + autopilot_get_mode() != AP_MODE_NAV && + autopilot_get_mode() != AP_MODE_MODULE && + autopilot_get_mode() != AP_MODE_FLIP && + autopilot_get_mode() != AP_MODE_GUIDED) { + autopilot_set_mode(RC_LOST_MODE); + } + +#if FAILSAFE_ON_BAT_CRITICAL + if (autopilot_get_mode() != AP_MODE_KILL && + electrical.bat_critical) { + autopilot_set_mode(AP_MODE_FAILSAFE); + } +#endif + +#if USE_GPS + if (autopilot_get_mode() == AP_MODE_NAV && + autopilot_get_motors_on() && +#if NO_GPS_LOST_WITH_RC_VALID + radio_control.status != RC_OK && +#endif +#ifdef NO_GPS_LOST_WITH_DATALINK_TIME + datalink_time > NO_GPS_LOST_WITH_DATALINK_TIME && +#endif + GpsIsLost()) { + autopilot_set_mode(AP_MODE_FAILSAFE); + } + + if (autopilot_get_mode() == AP_MODE_HOME && + autopilot_get_motors_on() && GpsIsLost()) { + autopilot_set_mode(AP_MODE_FAILSAFE); + } +#endif + +} + diff --git a/sw/airborne/firmwares/rotorcraft/main.h b/sw/airborne/firmwares/rotorcraft/main.h deleted file mode 100644 index 2db7f0cdbd..0000000000 --- a/sw/airborne/firmwares/rotorcraft/main.h +++ /dev/null @@ -1,46 +0,0 @@ -/* - * Copyright (C) 2008-2010 The Paparazzi Team - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file firmwares/rotorcraft/main.h - * - * Rotorcraft main loop. - */ - -#ifndef MAIN_H -#define MAIN_H - -#ifdef SITL -#define STATIC_INLINE extern -#else -#define STATIC_INLINE static inline -#endif - -STATIC_INLINE void main_init(void); -STATIC_INLINE void main_event(void); -STATIC_INLINE void handle_periodic_tasks(void); - -STATIC_INLINE void main_periodic(void); -STATIC_INLINE void telemetry_periodic(void); -STATIC_INLINE void failsafe_check(void); - - -#endif /* MAIN_H */ diff --git a/sw/airborne/firmwares/rotorcraft/main_ap.c b/sw/airborne/firmwares/rotorcraft/main_ap.c deleted file mode 100644 index 311ae1cd5a..0000000000 --- a/sw/airborne/firmwares/rotorcraft/main_ap.c +++ /dev/null @@ -1,260 +0,0 @@ -/* - * Copyright (C) 2008-2021 The Paparazzi Team - * - * This file is part of Paparazzi. - * - * Paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * Paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** - * @file firmwares/rotorcraft/main_ap.c - * - * Rotorcraft main loop. - */ - -#define MODULES_C - -#define ABI_C - -#include -#include "led.h" - -#include "modules/radio_control/radio_control.h" - -#include "firmwares/rotorcraft/main_ap.h" - -#ifdef SITL -#include "nps_autopilot.h" -#endif - -#include "generated/modules.h" -#include "modules/core/abi.h" - -/* if PRINT_CONFIG is defined, print some config options */ -PRINT_CONFIG_VAR(PERIODIC_FREQUENCY) -/* SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY should be an integer, otherwise the timer will not be correct */ -#if !(SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY*PERIODIC_FREQUENCY == SYS_TIME_FREQUENCY) -#warning "The SYS_TIME_FREQUENCY can not be divided by PERIODIC_FREQUENCY. Make sure this is the case for correct timing." -#endif - -/* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h - * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file - */ -PRINT_CONFIG_VAR(TELEMETRY_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" -INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY) -#endif -#endif - -/** - * IDs for timers - */ -tid_t modules_mcu_core_tid; // single step -tid_t modules_sensors_tid; -tid_t modules_radio_control_tid; -tid_t modules_gnc_tid; // estimation, control, actuators, default in a single step -tid_t modules_datalink_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) - -void main_init(void) -{ - modules_mcu_init(); - modules_core_init(); - modules_sensors_init(); - modules_estimation_init(); -#ifndef INTER_MCU_AP - radio_control_init(); - // modules_radio_control_init(); FIXME -#endif - modules_control_init(); - modules_actuators_init(); - modules_datalink_init(); - modules_default_init(); - - // register timers with temporal dependencies - 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); - failsafe_tid = sys_time_register_timer(0.05, NULL); // FIXME - -#if USE_IMU - // send body_to_imu from here for now - AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); -#endif - - // Do a failsafe check first - failsafe_check(); - -} - -void handle_periodic_tasks(void) -{ - if (sys_time_check_and_ack_timer(modules_sensors_tid)) { - modules_sensors_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_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_mcu_core_tid)) { - modules_mcu_periodic_task(); - modules_core_periodic_task(); - RunOnceEvery(10, LED_PERIODIC()); // FIXME periodic in led module - } - - if (sys_time_check_and_ack_timer(modules_datalink_tid)) { - telemetry_periodic(); - modules_datalink_periodic_task(); // FIXME integrate above -#if defined DATALINK || defined SITL - RunOnceEvery(TELEMETRY_FREQUENCY, datalink_time++); -#endif - } - - if (sys_time_check_and_ack_timer(failsafe_tid)) { - failsafe_check(); // FIXME integrate somewhere else - } - -} - -void telemetry_periodic(void) -{ - static uint8_t boot = true; - - /* initialisation phase during boot */ - if (boot) { -#if DOWNLINK - autopilot_send_version(); -#endif - boot = false; - } - /* then report periodicly */ - else { -#if PERIODIC_TELEMETRY - periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); -#endif - } -} - -/** mode to enter when RC is lost while using a mode with RC input (not AP_MODE_NAV) */ -#ifndef RC_LOST_MODE -#define RC_LOST_MODE AP_MODE_FAILSAFE -#endif - -void failsafe_check(void) -{ -#if !USE_GENERATED_AUTOPILOT - if (radio_control.status == RC_REALLY_LOST && - autopilot_get_mode() != AP_MODE_KILL && - autopilot_get_mode() != AP_MODE_HOME && - autopilot_get_mode() != AP_MODE_FAILSAFE && - autopilot_get_mode() != AP_MODE_NAV && - autopilot_get_mode() != AP_MODE_MODULE && - autopilot_get_mode() != AP_MODE_FLIP && - autopilot_get_mode() != AP_MODE_GUIDED) { - autopilot_set_mode(RC_LOST_MODE); - } - -#if FAILSAFE_ON_BAT_CRITICAL - if (autopilot_get_mode() != AP_MODE_KILL && - electrical.bat_critical) { - autopilot_set_mode(AP_MODE_FAILSAFE); - } -#endif - -#if USE_GPS - if (autopilot_get_mode() == AP_MODE_NAV && - autopilot_get_motors_on() && -#if NO_GPS_LOST_WITH_RC_VALID - radio_control.status != RC_OK && -#endif -#ifdef NO_GPS_LOST_WITH_DATALINK_TIME - datalink_time > NO_GPS_LOST_WITH_DATALINK_TIME && -#endif - GpsIsLost()) { - autopilot_set_mode(AP_MODE_FAILSAFE); - } - - if (autopilot_get_mode() == AP_MODE_HOME && - autopilot_get_motors_on() && GpsIsLost()) { - autopilot_set_mode(AP_MODE_FAILSAFE); - } -#endif - -#endif // !USE_GENERATED_AUTOPILOT - - autopilot_check_in_flight(autopilot_get_motors_on()); -} - -void main_event(void) -{ - modules_mcu_event_task(); - modules_core_event_task(); - modules_sensors_event_task(); - modules_estimation_event_task(); - modules_radio_control_event_task(); // FIXME - if (autopilot.use_rc) { - RadioControlEvent(autopilot_on_rc_frame); - } - modules_control_event_task(); - modules_actuators_event_task(); - modules_datalink_event_task(); - modules_default_event_task(); -} diff --git a/sw/airborne/firmwares/rotorcraft/main_ap.h b/sw/airborne/firmwares/rotorcraft/main_ap.h deleted file mode 100644 index 9fd0a6351a..0000000000 --- a/sw/airborne/firmwares/rotorcraft/main_ap.h +++ /dev/null @@ -1,40 +0,0 @@ -/* - * Copyright (C) 2008-2010 The Paparazzi Team - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file firmwares/rotorcraft/main_ap.h - * - * Rotorcraft main loop. - */ - -#ifndef MAIN_H -#define MAIN_H - -extern void main_init(void); -extern void main_event(void); -extern void handle_periodic_tasks(void); - -extern void main_periodic(void); -extern void telemetry_periodic(void); -extern void failsafe_check(void); - - -#endif /* MAIN_H */ diff --git a/sw/airborne/firmwares/rotorcraft/main_chibios.c b/sw/airborne/firmwares/rotorcraft/main_chibios.c deleted file mode 100644 index a0942ecc7f..0000000000 --- a/sw/airborne/firmwares/rotorcraft/main_chibios.c +++ /dev/null @@ -1,104 +0,0 @@ -/* - * Copyright (C) 2013-2015 Gautier Hattenberger, Alexandre Bustico - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file firmwares/fixedwing/main_chibios.c - */ - -#include "mcu_periph/sys_time.h" -#include "mcu.h" -#include - -#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 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 - -#include "firmwares/rotorcraft/main_ap.h" - -#ifndef THD_WORKING_AREA_MAIN -#define THD_WORKING_AREA_MAIN 8192 -#endif - -/* - * PPRZ/AP thread - * Also include FBW on single MCU - */ -static void thd_ap(void *arg); -THD_WORKING_AREA(wa_thd_ap, THD_WORKING_AREA_MAIN); -static thread_t *apThdPtr = NULL; - -/** - * Main function - */ -int main(void) -{ - // Init - main_init(); - - chThdSleepMilliseconds(100); - - // Create threads - apThdPtr = chThdCreateStatic(wa_thd_ap, sizeof(wa_thd_ap), NORMALPRIO, thd_ap, NULL); - - // Main loop, do nothing - chThdSleep(TIME_INFINITE); - return 0; -} - -/* - * PPRZ/AP thread - * - * Call PPRZ AP periodic and event functions - */ -static void thd_ap(void *arg) -{ - (void) arg; - chRegSetThreadName("AP"); - - while (!chThdShouldTerminateX()) { - systime_t t = chVTGetSystemTime(); - handle_periodic_tasks(); - main_event(); - // 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); -} - -/* - * Terminate autopilot threads - * Wait until proper stop - */ -void pprz_terminate_autopilot_threads(void) -{ - if (apThdPtr != NULL) { - chThdTerminate(apThdPtr); - chThdWait(apThdPtr); - apThdPtr = NULL; - } -} - diff --git a/sw/airborne/firmwares/rotorcraft/main_chibios.h b/sw/airborne/firmwares/rotorcraft/main_chibios.h deleted file mode 100644 index 19434ad16a..0000000000 --- a/sw/airborne/firmwares/rotorcraft/main_chibios.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * Copyright (C) 2013-2015 Gautier Hattenberger, Alexandre Bustico - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file firmwares/fixedwing/main_chibios.h - */ - -#ifndef MAIN_CHIBIOS_H -#define MAIN_CHIBIOS_H - -#include - -/** Terminate all autopilot threads - * Wait until proper stop - */ -extern void pprz_terminate_autopilot_threads(void); - -#endif /* MAIN_CHIBIOS_H */ diff --git a/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c b/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c index f92860584c..e7b3e5a127 100644 --- a/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c +++ b/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c @@ -43,6 +43,7 @@ void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct /* parse telemetry messages coming from ground station */ switch (msg_id) { +#ifndef INTERMCU_FBW #ifdef USE_NAVIGATION case DL_BLOCK : { @@ -82,6 +83,7 @@ void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct break; #endif +#endif // INTERMCU_FBW default: break; } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c index 2ebbaeee48..876f495e92 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c @@ -31,6 +31,7 @@ #include "modules/radio_control/radio_control.h" #include "generated/airframe.h" +#include "generated/modules.h" struct Int32Rates stabilization_none_rc_cmd; diff --git a/sw/airborne/firmwares/rover/autopilot_rc_helpers.h b/sw/airborne/firmwares/rover/autopilot_rc_helpers.h index 95e6571245..6b6ff3dcea 100644 --- a/sw/airborne/firmwares/rover/autopilot_rc_helpers.h +++ b/sw/airborne/firmwares/rover/autopilot_rc_helpers.h @@ -30,6 +30,7 @@ #include "generated/airframe.h" #include "modules/radio_control/radio_control.h" +#include "generated/modules.h" #define AUTOPILOT_THROTTLE_THRESHOLD (MAX_PPRZ / 20) #define AUTOPILOT_YAW_THRESHOLD (MAX_PPRZ * 19 / 20) diff --git a/sw/airborne/firmwares/rover/main_chibios.c b/sw/airborne/firmwares/rover/main_chibios.c deleted file mode 100644 index 0138b12f22..0000000000 --- a/sw/airborne/firmwares/rover/main_chibios.c +++ /dev/null @@ -1,105 +0,0 @@ -/* - * Copyright (C) 2018 Gautier Hattenberger, Alexandre Bustico - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - */ - -/** - * @file firmwares/rover/main_chibios.c - */ - -#include "mcu_periph/sys_time.h" -#include "mcu.h" -#include - -#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 CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY) -#error CH_CFG_ST_FREQUENCY and SYS_TIME_FREQUENCY should be >= 2 x PERIODIC_FREQUENCY -#endif - -#include "firmwares/rotorcraft/main_ap.h" - -#ifndef THD_WORKING_AREA_MAIN -#define THD_WORKING_AREA_MAIN 8192 -#endif - -/* - * PPRZ/AP thread - * Also include FBW on single MCU - */ -static void thd_ap(void *arg); -THD_WORKING_AREA(wa_thd_ap, THD_WORKING_AREA_MAIN); -static thread_t *apThdPtr = NULL; - -/** - * Main function - */ -int main(void) -{ - // Init - main_init(); - - chThdSleepMilliseconds(100); - - // Create threads - apThdPtr = chThdCreateStatic(wa_thd_ap, sizeof(wa_thd_ap), NORMALPRIO, thd_ap, NULL); - - // Main loop, do nothing - while (TRUE) { - chThdSleepMilliseconds(1000); - } - return 0; -} - -/* - * PPRZ/AP thread - * - * Call PPRZ AP periodic and event functions - */ -static void thd_ap(void *arg) -{ - (void) arg; - chRegSetThreadName("AP"); - - while (!chThdShouldTerminateX()) { - systime_t t = chVTGetSystemTime(); - handle_periodic_tasks(); - main_event(); - // 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); -} - -/* - * Terminate autopilot threads - * Wait until proper stop - */ -void pprz_terminate_autopilot_threads(void) -{ - if (apThdPtr != NULL) { - chThdTerminate(apThdPtr); - chThdWait(apThdPtr); - apThdPtr = NULL; - } -} - diff --git a/sw/airborne/firmwares/rover/main_ap.c b/sw/airborne/main_ap.c similarity index 67% rename from sw/airborne/firmwares/rover/main_ap.c rename to sw/airborne/main_ap.c index cbd6bbef72..d254261c55 100644 --- a/sw/airborne/firmwares/rover/main_ap.c +++ b/sw/airborne/main_ap.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 Gautier Hattenberger + * Copyright (C) 2008-2021 The Paparazzi Team * * This file is part of Paparazzi. * @@ -20,25 +20,27 @@ */ /** - * @file firmwares/rover/main_ap.c + * @file main_ap.c * - * Rover main loop. + * Autopilot main loop. + * + * This process is reponsible for the collecting the different sensors data, + * calling the appropriate estimation algorithms and running the different control loops. */ #define MODULES_C #define ABI_C -#include -#include "led.h" - -#include "modules/radio_control/radio_control.h" - -#include "firmwares/rover/main_ap.h" - +#include "main_ap.h" +#include "generated/airframe.h" #include "generated/modules.h" #include "modules/core/abi.h" +#ifdef USE_NPS +#include "nps_autopilot.h" +#endif + /* if PRINT_CONFIG is defined, print some config options */ PRINT_CONFIG_VAR(PERIODIC_FREQUENCY) /* SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY should be an integer, otherwise the timer will not be correct */ @@ -66,21 +68,19 @@ tid_t modules_sensors_tid; tid_t modules_radio_control_tid; 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) -void main_init(void) +void main_ap_init(void) { - modules_mcu_init(); + // mcu init done in main + modules_core_init(); modules_sensors_init(); modules_estimation_init(); - radio_control_init(); - // modules_radio_control_init(); FIXME + modules_radio_control_init(); modules_control_init(); modules_actuators_init(); modules_datalink_init(); @@ -107,7 +107,6 @@ void main_init(void) 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); - failsafe_tid = sys_time_register_timer(0.05, NULL); // FIXME #if USE_IMU // send body_to_imu from here for now @@ -115,91 +114,44 @@ void main_init(void) #endif // Do a failsafe check first - failsafe_check(); + autopilot_failsafe_checks(); } -void handle_periodic_tasks(void) +void main_ap_periodic(void) { if (sys_time_check_and_ack_timer(modules_sensors_tid)) { modules_sensors_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 + modules_radio_control_periodic_task(); } 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 ? - } + modules_actuators_periodic_task(); } if (sys_time_check_and_ack_timer(modules_mcu_core_tid)) { modules_mcu_periodic_task(); modules_core_periodic_task(); - RunOnceEvery(10, LED_PERIODIC()); // FIXME periodic in led module } if (sys_time_check_and_ack_timer(modules_datalink_tid)) { - telemetry_periodic(); - modules_datalink_periodic_task(); // FIXME integrate above -#if defined DATALINK || defined SITL - RunOnceEvery(TELEMETRY_FREQUENCY, datalink_time++); -#endif - } - - if (sys_time_check_and_ack_timer(failsafe_tid)) { - failsafe_check(); // FIXME integrate somewhere else - } - -} - -void telemetry_periodic(void) -{ - static uint8_t boot = true; - - /* initialisation phase during boot */ - if (boot) { -#if DOWNLINK - autopilot_send_version(); -#endif - boot = false; - } - /* then report periodicly */ - else { -#if PERIODIC_TELEMETRY - periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); -#endif + modules_datalink_periodic_task(); } } -/** mode to enter when RC is lost while using a mode with RC input (not AP_MODE_NAV) */ -#ifndef RC_LOST_MODE -#define RC_LOST_MODE AP_MODE_FAILSAFE -#endif - -void failsafe_check(void) -{ - autopilot_check_in_flight(autopilot_get_motors_on()); -} - -void main_event(void) +void main_ap_event(void) { modules_mcu_event_task(); modules_core_event_task(); modules_sensors_event_task(); modules_estimation_event_task(); - modules_radio_control_event_task(); // FIXME - if (autopilot.use_rc) { - RadioControlEvent(autopilot_on_rc_frame); - } + modules_radio_control_event_task(); modules_control_event_task(); modules_actuators_event_task(); modules_datalink_event_task(); diff --git a/sw/airborne/main_ap.h b/sw/airborne/main_ap.h new file mode 100644 index 0000000000..f34dfb555a --- /dev/null +++ b/sw/airborne/main_ap.h @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2008-2021 The Paparazzi Team + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + * + */ + +/** + * @file main_ap.h + * + * Autopilot main loop. + * + * This process is reponsible for the collecting the different sensors data, + * calling the appropriate estimation algorithms and running the different control loops. + */ + +#ifndef MAIN_AP_H +#define MAIN_AP_H + +extern void main_ap_init(void); +extern void main_ap_periodic(void); +extern void main_ap_event(void); + +#endif /* MAIN_AP_H */ + diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/main_bare.c similarity index 68% rename from sw/airborne/firmwares/rotorcraft/main.c rename to sw/airborne/main_bare.c index 826cb3193f..a213c11dbe 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/main_bare.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Gautier Hattenberger + * Copyright (C) 2017-2021 Gautier Hattenberger * * This file is part of Paparazzi. * @@ -20,28 +20,45 @@ */ /** - * @file firmwares/rotorcraft/main.c + * @file main_bare.c * - * Program main function - * AP + FBW on single MCU - * AP or FBW on dual MCU + * Program main function with baremetal inplementation (no RTOS) + * + * Calls AP on single/dual MCU + * Limit polling when used with an OS (e.g. Linux) * None on SITL */ -#if FBW -#include "firmwares/rotorcraft/main_fbw.h" -#else -#include "firmwares/rotorcraft/main_ap.h" +#if (defined AP) && (defined FBW) +#error "AP and FBW can't be defined at the same time" #endif +#if (!defined AP) && (!defined FBW) +#error "AP or FBW should be defined" +#endif + +#ifdef FBW +#include "main_fbw.h" +#define Call(f) main_fbw_ ## f +#endif + +#ifdef AP +#include "main_ap.h" +#define Call(f) main_ap_ ## f +#endif + +#include "generated/modules.h" #include "mcu_periph/sys_time.h" #define POLLING_PERIOD (500000/PERIODIC_FREQUENCY) -#include + #ifndef SITL int main(void) { - main_init(); + // mcu modules init in all cases + modules_mcu_init(); + + Call(init()); #if LIMIT_EVENT_POLLING /* Limit main loop frequency to 1kHz. @@ -55,8 +72,8 @@ int main(void) while (1) { t_begin = get_sys_time_usec(); - handle_periodic_tasks(); - main_event(); + Call(periodic()); + Call(event()); /* sleep remaining time to limit to polling frequency */ t_diff = get_sys_time_usec() - t_begin; @@ -66,8 +83,8 @@ int main(void) } #else while (1) { - handle_periodic_tasks(); - main_event(); + Call(periodic()); + Call(event()); } #endif diff --git a/sw/airborne/modules/intermcu/link_mcu.h b/sw/airborne/main_bare.h similarity index 50% rename from sw/airborne/modules/intermcu/link_mcu.h rename to sw/airborne/main_bare.h index 9642527e17..746503fab1 100644 --- a/sw/airborne/modules/intermcu/link_mcu.h +++ b/sw/airborne/main_bare.h @@ -1,14 +1,14 @@ /* - * Copyright (C) 2003-2014 The Paparazzi Team + * Copyright (C) 2005-2021 The Paparazzi Team * - * This file is part of paparazzi. + * This file is part of Paparazzi. * - * paparazzi is free software; you can redistribute it and/or modify + * Paparazzi is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2, or (at your option) * any later version. * - * paparazzi is distributed in the hope that it will be useful, + * Paparazzi is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. @@ -16,22 +16,14 @@ * You should have received a copy of the GNU General Public License * along with paparazzi; see the file COPYING. If not, see * . + * */ /** - * @file modules/intermcu/link_mcu.h - * Common transport functions for the communication between FBW and AP. + * @file main_bare.h */ -#ifndef LINK_MCU_H -#define LINK_MCU_H +#ifndef MAIN_BARE_H +#define MAIN_BARE_H -#ifdef MCU_SPI_LINK -#include "link_mcu_spi.h" -#elif defined MCU_UART_LINK -#include "link_mcu_usart.h" -#elif defined MCU_CAN_LINK -#include "link_mcu_can.h" -#endif - -#endif /* LINK_MCU_H */ +#endif /* MAIN_BARE_H */ diff --git a/sw/airborne/firmwares/fixedwing/main_chibios.c b/sw/airborne/main_chibios.c similarity index 60% rename from sw/airborne/firmwares/fixedwing/main_chibios.c rename to sw/airborne/main_chibios.c index 8a27ab45c7..0bf65c65ff 100644 --- a/sw/airborne/firmwares/fixedwing/main_chibios.c +++ b/sw/airborne/main_chibios.c @@ -1,26 +1,30 @@ /* - * Copyright (C) 2013-2015 Gautier Hattenberger, Alexandre Bustico + * Copyright (C) 2013-2021 Gautier Hattenberger, Alexandre Bustico * - * This file is part of paparazzi. + * This file is part of Paparazzi. * - * paparazzi is free software; you can redistribute it and/or modify + * Paparazzi is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2, or (at your option) * any later version. * - * paparazzi is distributed in the hope that it will be useful, + * Paparazzi is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . + * */ /** - * @file firmwares/fixedwing/main_chibios.c + * @file main_chibios.c + * + * Program main function for ChibiOS inplementation + * + * Calls AP thread on single/dual MCU */ #include "mcu_periph/sys_time.h" @@ -33,51 +37,47 @@ #error CH_CFG_ST_FREQUENCY and SYS_TIME_FREQUENCY should be >= 2 x PERIODIC_FREQUENCY #endif +#if (defined AP) && (defined FBW) +#error "AP and FBW can't be defined at the same time" +#endif +#if (!defined AP) && (!defined FBW) +#error "AP or FBW should be defined" +#endif + #ifdef FBW -#include "firmwares/fixedwing/main_fbw.h" -#define Fbw(f) f ## _fbw() -#else -#define Fbw(f) +#include "main_fbw.h" +#define Call(f) main_fbw_ ## f #endif #ifdef AP -#include "firmwares/fixedwing/main_ap.h" -#define Ap(f) f ## _ap() -#else -#define Ap(f) +#include "main_ap.h" +#define Call(f) main_ap_ ## f #endif -#if USE_HARD_FAULT_RECOVERY -#include "modules/datalink/downlink.h" -#endif +#include "generated/modules.h" -/* - * Default autopilot thread stack size - */ -#ifndef AP_THREAD_STACK_SIZE -#define AP_THREAD_STACK_SIZE 8192 +#ifndef THD_WORKING_AREA_MAIN +#define THD_WORKING_AREA_MAIN 8192 #endif /* * PPRZ/AP thread */ static void thd_ap(void *arg); -static THD_WORKING_AREA(wa_thd_ap, AP_THREAD_STACK_SIZE); +THD_WORKING_AREA(wa_thd_ap, THD_WORKING_AREA_MAIN); static thread_t *apThdPtr = NULL; -/* - * Default FBW thread stack size - */ -#ifndef FBW_THREAD_STACK_SIZE -#define FBW_THREAD_STACK_SIZE 1024 +#if USE_HARD_FAULT_RECOVERY +#include "main_recovery.h" + +#ifndef THD_WORKING_AREA_RECOVERY +#define THD_WORKING_AREA_RECOVERY 1024 #endif -/* - * PPRZ/FBW thread - */ -static void thd_fbw(void *arg); -static THD_WORKING_AREA(wa_thd_fbw, FBW_THREAD_STACK_SIZE); -static thread_t *fbwThdPtr = NULL; +static void thd_recovery(void *arg); +THD_WORKING_AREA(wa_thd_recovery, THD_WORKING_AREA_RECOVERY); +static thread_t *recoveryThdPtr = NULL; +#endif /** * Main function @@ -85,30 +85,27 @@ static thread_t *fbwThdPtr = NULL; int main(void) { // Init - Fbw(init); + + // mcu modules init in all cases + modules_mcu_init(); + #if USE_HARD_FAULT_RECOVERY // if recovering from hard fault, don't call AP init, only FBW if (!recovering_from_hard_fault) { #endif - Ap(init); + Call(init()); + chThdSleepMilliseconds(100); + + // Create threads + apThdPtr = chThdCreateStatic(wa_thd_ap, sizeof(wa_thd_ap), NORMALPRIO, thd_ap, NULL); + #if USE_HARD_FAULT_RECOVERY } else { - // but we still need downlink to be initialized - downlink_init(); - modules_datalink_init(); - } -#endif + main_recovery_init(); + chThdSleepMilliseconds(100); - chThdSleepMilliseconds(100); - - // Create threads - fbwThdPtr = chThdCreateStatic(wa_thd_fbw, sizeof(wa_thd_fbw), NORMALPRIO, thd_fbw, NULL); -#if USE_HARD_FAULT_RECOVERY - // if recovering from hard fault, don't start AP thread, only FBW - if (!recovering_from_hard_fault) { -#endif - apThdPtr = chThdCreateStatic(wa_thd_ap, sizeof(wa_thd_ap), NORMALPRIO, thd_ap, NULL); -#if USE_HARD_FAULT_RECOVERY + // Create threads + recoveryThdPtr = chThdCreateStatic(wa_thd_recovery, sizeof(wa_thd_recovery), NORMALPRIO, thd_recovery, NULL); } #endif @@ -129,8 +126,8 @@ static void thd_ap(void *arg) while (!chThdShouldTerminateX()) { systime_t t = chVTGetSystemTime(); - Ap(handle_periodic_tasks); - Ap(event_task); + Call(periodic()); + Call(event()); // 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 @@ -141,20 +138,21 @@ static void thd_ap(void *arg) chThdExit(0); } +#if USE_HARD_FAULT_RECOVERY /* - * PPRZ/FBW thread + * PPRZ/RECOVERY thread * - * Call PPRZ FBW periodic and event functions + * Call PPRZ minimal AP after MCU hard fault */ -static void thd_fbw(void *arg) +static void thd_recovery(void *arg) { (void) arg; - chRegSetThreadName("FBW"); + chRegSetThreadName("RECOVERY"); while (!chThdShouldTerminateX()) { systime_t t = chVTGetSystemTime(); - Fbw(handle_periodic_tasks); - Fbw(event_task); + main_recovery_periodic(); + main_recovery_event(); // 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 @@ -164,6 +162,7 @@ static void thd_fbw(void *arg) chThdExit(0); } +#endif /* * Terminate autopilot threads @@ -176,10 +175,12 @@ void pprz_terminate_autopilot_threads(void) chThdWait(apThdPtr); apThdPtr = NULL; } - if (fbwThdPtr != NULL) { - chThdTerminate(fbwThdPtr); - chThdWait(fbwThdPtr); - fbwThdPtr = NULL; +#if USE_HARD_FAULT_RECOVERY + if (recoveryThdPtr != NULL) { + chThdTerminate(recoveryThdPtr); + chThdWait(recoveryThdPtr); + recoveryThdPtr = NULL; } +#endif } diff --git a/sw/airborne/firmwares/rover/main_chibios.h b/sw/airborne/main_chibios.h similarity index 73% rename from sw/airborne/firmwares/rover/main_chibios.h rename to sw/airborne/main_chibios.h index 0744c70e6a..3ffbb7a907 100644 --- a/sw/airborne/firmwares/rover/main_chibios.h +++ b/sw/airborne/main_chibios.h @@ -1,14 +1,14 @@ /* - * Copyright (C) 2018 Gautier Hattenberger, Alexandre Bustico + * Copyright (C) 2013-2021 Gautier Hattenberger, Alexandre Bustico * - * This file is part of paparazzi. + * This file is part of Paparazzi. * - * paparazzi is free software; you can redistribute it and/or modify + * Paparazzi is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2, or (at your option) * any later version. * - * paparazzi is distributed in the hope that it will be useful, + * Paparazzi is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. @@ -16,10 +16,11 @@ * You should have received a copy of the GNU General Public License * along with paparazzi; see the file COPYING. If not, see * . + * */ /** - * @file firmwares/rover/main_chibios.h + * @file main_chibios.h */ #ifndef MAIN_CHIBIOS_H diff --git a/sw/airborne/firmwares/rotorcraft/main_fbw.c b/sw/airborne/main_fbw.c similarity index 62% rename from sw/airborne/firmwares/rotorcraft/main_fbw.c rename to sw/airborne/main_fbw.c index 6afca50c77..56f07bb007 100644 --- a/sw/airborne/firmwares/rotorcraft/main_fbw.c +++ b/sw/airborne/main_fbw.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2015 The Paparazzi Team + * Copyright (C) 2022 Gautier Hattenberger * * This file is part of Paparazzi. * @@ -14,37 +15,27 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with Paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ /** - * @file firmwares/rotorcraft/main_fbw.c + * @file main_fbw.c * - * Rotorcraft FBW main loop. + * FBW main loop. */ -#include -#include "mcu.h" -#include "led.h" -#include "mcu_periph/sys_time.h" - -#include "modules/core/commands.h" -#include "modules/actuators/actuators.h" -#if USE_MOTOR_MIXING -#include "modules/actuators/motor_mixing.h" -#endif - -#include "modules/energy/electrical.h" -#include "modules/radio_control/radio_control.h" -#include "modules/intermcu/intermcu_fbw.h" -#include "firmwares/rotorcraft/main_fbw.h" -#include "firmwares/rotorcraft/autopilot_rc_helpers.h" - #define MODULES_C + +#define ABI_C + +#include "main_fbw.h" +#include "generated/airframe.h" #include "generated/modules.h" +#include "modules/core/abi.h" +#include "modules/datalink/datalink.h" +#include "modules/datalink/telemetry.h" /* So one can use these in command_laws section */ #define And(x, y) ((x) && (y)) @@ -56,83 +47,69 @@ /** Fly by wire modes */ -fbw_mode_enum fbw_mode; +uint8_t fbw_mode; bool fbw_motors_on = false; -/* MODULES_FREQUENCY is defined in generated/modules.h - * according to main_freq parameter set for modules in airframe file +/* if PRINT_CONFIG is defined, print some config options */ +PRINT_CONFIG_VAR(PERIODIC_FREQUENCY) +/* SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY should be an integer, otherwise the timer will not be correct */ +#if !(SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY*PERIODIC_FREQUENCY == SYS_TIME_FREQUENCY) +#warning "The SYS_TIME_FREQUENCY can not be divided by PERIODIC_FREQUENCY. Make sure this is the case for correct timing." +#endif + +/* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h + * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file */ -PRINT_CONFIG_VAR(MODULES_FREQUENCY) +PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY) -tid_t main_periodic_tid; ///< id for main_periodic() timer -tid_t modules_tid; ///< id for modules_periodic_task() timer -tid_t radio_control_tid; ///< id for radio_control_periodic_task() timer -tid_t electrical_tid; ///< id for electrical_periodic() timer -tid_t telemetry_tid; ///< id for telemetry_periodic() timer +/** + * IDs for timers + */ +tid_t periodic_tid; ///< id for general periodic task timer +tid_t radio_control_tid; ///< id for radio_control_periodic() timer +tid_t electrical_tid; ///< id for electrical_periodic() timer +tid_t telemetry_tid; ///< id for telemetry_periodic() timer + +/** + * ABI RC binding + */ +#ifndef MAIN_FBW_RC_ID +#define MAIN_FBW_RC_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(MAIN_FBW_RC_ID) +static abi_event rc_ev; +static void rc_cb(uint8_t sender_id, struct RadioControl *rc); -/** Main initialization */ -void main_init(void) +/** + * Main initialization + */ +void main_fbw_init(void) { + // mcu init done in main + + modules_core_init(); + modules_radio_control_init(); + modules_actuators_init(); + modules_datalink_init(); + // Set startup mode to Failsafe fbw_mode = FBW_MODE_FAILSAFE; - mcu_init(); - - actuators_init(); - - electrical_init(); - -#if USE_MOTOR_MIXING - motor_mixing_init(); -#endif - - radio_control_init(); - - modules_init(); - - - intermcu_init(); + // Bind to RC event + AbiBindMsgRADIO_CONTROL(MAIN_FBW_RC_ID, &rc_ev, rc_cb); // Register the timers for the periodic functions - main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); - modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL); + periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); radio_control_tid = sys_time_register_timer((1. / 60.), NULL); electrical_tid = sys_time_register_timer(0.1, NULL); - telemetry_tid = sys_time_register_timer((1. / 10.), NULL); + telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL); } -////////////////////////// -// PERIODIC - -void handle_periodic_tasks(void) -{ - if (sys_time_check_and_ack_timer(main_periodic_tid)) { - main_periodic(); - } - if (sys_time_check_and_ack_timer(modules_tid)) { - modules_periodic_task(); - } - if (sys_time_check_and_ack_timer(radio_control_tid)) { - radio_control_periodic_task(); - } - if (sys_time_check_and_ack_timer(electrical_tid)) { - electrical_periodic(); - } - if (sys_time_check_and_ack_timer(telemetry_tid)) { - telemetry_periodic(); - } -} - -void telemetry_periodic(void) -{ - /* Send status to AP */ - intermcu_send_status(fbw_mode); - - /* Handle Modems */ - // TODO -} +/** + * Periodic tasks + */ /* Checks the different safety cases and sets the correct FBW mode */ static void fbw_safety_check(void) @@ -171,11 +148,8 @@ static void fbw_safety_check(void) } /* Sets the actual actuator commands */ -void main_periodic(void) +static void main_task_periodic(void) { - /* Inter-MCU watchdog */ - intermcu_periodic(); - /* Safety check and set FBW mode */ fbw_safety_check(); @@ -210,16 +184,35 @@ void main_periodic(void) SetCommands(commands_failsafe); } - /* If in auto copy autopilot motors on */ + /* If in auto copy autopilot motors on and commands from intermcu */ if (fbw_mode == FBW_MODE_AUTO) { - fbw_motors_on = autopilot_motors_on; + fbw_motors_on = intermcu_ap_motors_on; + SetCommands(intermcu_commands); } - /* Set actuators */ - SetActuatorsFromCommands(commands, autopilot_get_mode()); + /* in MANUAL, commands are updated in RC callback */ +} - /* Periodic blinking */ - RunOnceEvery(10, LED_PERIODIC()); +void main_fbw_periodic(void) +{ + if (sys_time_check_and_ack_timer(radio_control_tid)) { + modules_radio_control_periodic_task(); + } + + if (sys_time_check_and_ack_timer(periodic_tid)) { + main_task_periodic(); + modules_actuators_periodic_task(); + modules_mcu_periodic_task(); + modules_core_periodic_task(); + } + + if (sys_time_check_and_ack_timer(electrical_tid)) { + electrical_periodic(); + } + + if (sys_time_check_and_ack_timer(telemetry_tid)) { + modules_datalink_periodic_task(); + } } @@ -227,7 +220,7 @@ void main_periodic(void) // Event /** Callback when we received an RC frame */ -static void fbw_on_rc_frame(void) +static void rc_cb(uint8_t sender_id __attribute__((unused)), struct RadioControl *rc __attribute__((unused))) { /* get autopilot fbw mode as set by RADIO_MODE 3-way switch */ if (radio_control.values[RADIO_FBW_MODE] < (MIN_PPRZ / 2) && !FBW_MODE_AUTO_ONLY) { @@ -267,31 +260,20 @@ static void fbw_on_rc_frame(void) #warning "FBW: needs commands from RC in order to be useful." #endif } - - /* Forward radiocontrol to AP */ - intermcu_on_rc_frame(fbw_mode); } -/** Callback when receive commands from the AP */ -static void fbw_on_ap_command(void) +void main_fbw_parse_EMERGENCY_CMD(uint8_t *buf) { - // Only set the command from AP when we are in AUTO mode - if (fbw_mode == FBW_MODE_AUTO) { - SetCommands(intermcu_commands); + if (DL_EMERGENCY_CMD_ac_id(buf) == AC_ID && DL_EMERGENCY_CMD_cmd(buf) == 0) { + fbw_mode = FBW_MODE_FAILSAFE; } } -void main_event(void) +void main_fbw_event(void) { - /* Event functions for mcu peripherals: i2c, usb_serial.. */ - mcu_event(); - - /* Handle RC */ - RadioControlEvent(fbw_on_rc_frame); - - /* InterMCU (gives autopilot commands as output) */ - InterMcuEvent(fbw_on_ap_command); - - /* FBW modules */ - modules_event_task(); + modules_mcu_event_task(); + intermcu_event(); + modules_radio_control_event_task(); + modules_actuators_event_task(); + modules_datalink_event_task(); } diff --git a/sw/airborne/firmwares/rotorcraft/main_fbw.h b/sw/airborne/main_fbw.h similarity index 67% rename from sw/airborne/firmwares/rotorcraft/main_fbw.h rename to sw/airborne/main_fbw.h index 8f245e74b9..df74a505b5 100644 --- a/sw/airborne/firmwares/rotorcraft/main_fbw.h +++ b/sw/airborne/main_fbw.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2015 The Paparazzi Team + * Copyright (C) 2022 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,31 +15,34 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . + * */ /** - * @file firmwares/rotorcraft/main_fbw.h + * @file main_fbw.h * * Fly By Wire: * - * Reads Radiocontrol - * Reads Intermcu - * Sets Actuators + * Reads radio_control + * Reads intermcu + * Sets actuators + * Run datalink/telemetry * * if no rc but autopilot then RC_LOST_FBW_MODE (define below) * if no rc while in auto mode then RC_LOST_IN_AUTO_FBW_MODE (define below) * if no ap but rc then AP_LOST_FBW_MODE (define below) */ -#ifndef MAIN_H -#define MAIN_H +#ifndef MAIN_FBW_H +#define MAIN_FBW_H -/** mode to enter when RC is lost while using a mode with RC input */ +/** mode to enter when RC is lost while using a mode with RC input + * switching to AUTO allows a recover with HOME mode + */ #ifndef RC_LOST_FBW_MODE -#define RC_LOST_FBW_MODE FBW_MODE_FAILSAFE +#define RC_LOST_FBW_MODE FBW_MODE_AUTO #endif /** mode to enter when AP is lost while using autopilot */ @@ -61,13 +65,19 @@ #define RADIO_FBW_MODE RADIO_MODE #endif -typedef enum {FBW_MODE_MANUAL = 0, FBW_MODE_AUTO = 1, FBW_MODE_FAILSAFE = 2} fbw_mode_enum; +#define FBW_MODE_MANUAL 0 +#define FBW_MODE_AUTO 1 +#define FBW_MODE_FAILSAFE 2 +#include "std.h" -extern void main_init(void); -extern void main_event(void); -extern void handle_periodic_tasks(void); -extern void main_periodic(void); -extern void telemetry_periodic(void); +extern uint8_t fbw_mode; +extern bool fbw_motors_on; + +extern void main_fbw_init(void); +extern void main_fbw_event(void); +extern void main_fbw_periodic(void); +extern void main_fbw_parse_EMERGENCY_CMD(uint8_t *buf); + +#endif /* MAIN_FBW_H */ -#endif /* MAIN_H */ diff --git a/sw/airborne/mcu.c b/sw/airborne/mcu.c index b2671b42dd..3aaef67264 100644 --- a/sw/airborne/mcu.c +++ b/sw/airborne/mcu.c @@ -35,8 +35,8 @@ #include "led.h" #endif #if defined RADIO_CONTROL -#if defined RADIO_CONTROL_BIND_IMPL_FUNC & defined SPEKTRUM_BIND_PIN_PORT -#include "modules/radio_control/radio_control.h" +#if defined RADIO_CONTROL_BIND_IMPL_FUNC && defined SPEKTRUM_BIND_PIN_PORT +#include "modules/radio_control/spektrum.h" #endif #endif #if USE_UART0 || USE_UART1 || USE_UART2 || USE_UART3 || USE_UART4 || USE_UART5 || USE_UART6 || USE_UART7 || USE_UART8 @@ -132,7 +132,7 @@ void mcu_init(void) PERIPHERAL3V3_ENABLE_ON(PERIPHERAL3V3_ENABLE_PORT, PERIPHERAL3V3_ENABLE_PIN); #endif /* for now this means using spektrum */ -#if defined RADIO_CONTROL & defined RADIO_CONTROL_BIND_IMPL_FUNC & defined SPEKTRUM_BIND_PIN_PORT +#if defined RADIO_CONTROL && defined RADIO_CONTROL_BIND_IMPL_FUNC && defined SPEKTRUM_BIND_PIN_PORT RADIO_CONTROL_BIND_IMPL_FUNC(); #endif #if USE_UART0 diff --git a/sw/airborne/modules/actuators/actuators.c b/sw/airborne/modules/actuators/actuators.c index 35cfc88ed7..6d2ca0bc32 100644 --- a/sw/airborne/modules/actuators/actuators.c +++ b/sw/airborne/modules/actuators/actuators.c @@ -26,10 +26,26 @@ */ #include "modules/actuators/actuators.h" +#include "modules/core/commands.h" #include "mcu_periph/sys_time.h" +#ifdef INTERMCU_AP +#include "modules/intermcu/intermcu_ap.h" +#endif +#ifdef INTERMCU_FBW +#include "main_fbw.h" +#endif #if ACTUATORS_NB +#if PERIODIC_TELEMETRY +#include "modules/datalink/telemetry.h" + +static void send_actuators(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators); +} +#endif + int16_t actuators[ACTUATORS_NB]; // Can be used to directly control each actuator from the control algorithm @@ -50,8 +66,58 @@ void actuators_init(void) #endif // Init macro from generated airframe.h +#if (defined INTERMCU_AP) + // TODO ApOnlyActuatorsInit(); +#elif (defined INTERMCU_FBW) AllActuatorsInit(); +#else + // default, init all actuators + AllActuatorsInit(); + // TODO ApOnlyActuatorsInit(); +#endif +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators); +#endif } +/** Actuators periodic + * + * Set actuators from trimmed commands + */ +void actuators_periodic(void) +{ + pprz_t trimmed_commands[COMMANDS_NB]; + int i; + for (i = 0; i < COMMANDS_NB; i++) {trimmed_commands[i] = commands[i];} + +#ifdef COMMAND_ROLL + trimmed_commands[COMMAND_ROLL] += ClipAbs(command_roll_trim, MAX_PPRZ / 10); +#endif /* COMMAND_ROLL */ + +#ifdef COMMAND_PITCH + trimmed_commands[COMMAND_PITCH] += ClipAbs(command_pitch_trim, MAX_PPRZ / 10); +#endif /* COMMAND_PITCH */ + +#ifdef COMMAND_YAW + trimmed_commands[COMMAND_YAW] += ClipAbs(command_yaw_trim, MAX_PPRZ); +#endif /* COMMAND_YAW */ + +#if (defined INTERMCU_AP) + intermcu_send_commands(trimmed_commands, autopilot_get_mode()); + // TODO SetApOnlyActuatorsFromCommands(ap_commands, autopilot_get_mode()); +#elif (defined INTERMCU_FBW) + SetActuatorsFromCommands(trimmed_commands, autopilot_get_mode()); +#else + // default, apply all commands + SetActuatorsFromCommands(trimmed_commands, autopilot_get_mode()); + // TODO SetApOnlyActuatorsFromCommands(ap_commands, autopilot_get_mode()); +#endif +} + +#else // No command_laws section or no actuators + +void actuators_init(void) {} +void actuators_periodic(void) {} + #endif diff --git a/sw/airborne/modules/actuators/actuators.h b/sw/airborne/modules/actuators/actuators.h index 411a99f7e8..d1b38eba2d 100644 --- a/sw/airborne/modules/actuators/actuators.h +++ b/sw/airborne/modules/actuators/actuators.h @@ -37,9 +37,10 @@ */ #include "generated/airframe.h" -#if ACTUATORS_NB - extern void actuators_init(void); +extern void actuators_periodic(void); + +#if ACTUATORS_NB extern uint32_t actuators_delay_time; extern bool actuators_delay_done; diff --git a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c index c74c48f886..c74fcfdeb3 100644 --- a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c +++ b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c @@ -19,7 +19,7 @@ #include #include -#include "modules/intermcu/inter_mcu.h" +#include "modules/core/commands.h" #include "modules/nav/common_nav.h" #include "autopilot.h" #include "generated/flight_plan.h" @@ -170,7 +170,7 @@ void airborne_ant_point_periodic(void) airborne_ant_pan_servo = TRIM_PPRZ(airborne_ant_pan_servo); #ifdef COMMAND_ANT_PAN - imcu_set_command(COMMAND_ANT_PAN, airborne_ant_pan_servo); + command_set(COMMAND_ANT_PAN, airborne_ant_pan_servo); #endif diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c index c5674692b6..bf5dcbcd1a 100644 --- a/sw/airborne/modules/cam_control/cam.c +++ b/sw/airborne/modules/cam_control/cam.c @@ -193,7 +193,7 @@ void cam_periodic(void) #if defined(COMMAND_CAM_PWR_SW) - if (video_tx_state) { imcu_set_command(COMMAND_CAM_PWR_SW, MAX_PPRZ); } else { imcu_set_command(COMMAND_CAM_PWR_SW, MIN_PPRZ); } + if (video_tx_state) { command_set(COMMAND_CAM_PWR_SW, MAX_PPRZ); } else { command_set(COMMAND_CAM_PWR_SW, MIN_PPRZ); } #elif defined(VIDEO_TX_SWITCH) if (video_tx_state) { LED_OFF(VIDEO_TX_SWITCH); } else { LED_ON(VIDEO_TX_SWITCH); } #endif @@ -250,10 +250,10 @@ void cam_angles(void) cam_theta_c = cam_tilt_c; #ifdef COMMAND_CAM_PAN - imcu_set_command(COMMAND_CAM_PAN, cam_pan); + command_set(COMMAND_CAM_PAN, cam_pan); #endif #ifdef COMMAND_CAM_TILT - imcu_set_command(COMMAND_CAM_TILT, cam_tilt); + command_set(COMMAND_CAM_TILT, cam_tilt); #endif } diff --git a/sw/airborne/modules/cam_control/cam.h b/sw/airborne/modules/cam_control/cam.h index 3e2509cc3d..4d15fcd7af 100644 --- a/sw/airborne/modules/cam_control/cam.h +++ b/sw/airborne/modules/cam_control/cam.h @@ -28,7 +28,7 @@ #define CAM_H #include -#include "modules/intermcu/inter_mcu.h" +#include "modules/core/commands.h" #define CAM_MODE_OFF 0 /* Do nothing */ #define CAM_MODE_ANGLES 1 /* Input: servo angles */ @@ -75,9 +75,9 @@ void cam_periodic(void); void cam_init(void); extern int16_t cam_pan_command; -#define cam_SetPanCommand(x) { cam_pan_command = x; imcu_set_command(COMMAND_CAM_PAN, cam_pan_command);} +#define cam_SetPanCommand(x) { cam_pan_command = x; command_set(COMMAND_CAM_PAN, cam_pan_command);} extern int16_t cam_tilt_command; -#define cam_SetTiltCommand(x) { cam_tilt_command = x; imcu_set_command(COMMAND_CAM_TILT, cam_tilt_command);} +#define cam_SetTiltCommand(x) { cam_tilt_command = x; command_set(COMMAND_CAM_TILT, cam_tilt_command);} #ifdef TEST_CAM extern float test_cam_estimator_x; diff --git a/sw/airborne/modules/cam_control/cam_roll.c b/sw/airborne/modules/cam_control/cam_roll.c index e9d431fbe4..c3a75b4f2a 100644 --- a/sw/airborne/modules/cam_control/cam_roll.c +++ b/sw/airborne/modules/cam_control/cam_roll.c @@ -31,7 +31,7 @@ #include "autopilot.h" #include "generated/flight_plan.h" #include "state.h" -#include "modules/intermcu/inter_mcu.h" +#include "modules/core/commmands.h" #ifndef CAM_PHI_MAX #define CAM_PHI_MAX RadOfDeg(45) @@ -71,7 +71,7 @@ void cam_periodic(void) default: phi_c = 0; } - imcu_set_command(COMMAND_CAM_ROLL, TRIM_PPRZ(phi_c * MAX_PPRZ / CAM_PHI_MAX)); + command_set(COMMAND_CAM_ROLL, TRIM_PPRZ(phi_c * MAX_PPRZ / CAM_PHI_MAX)); } #endif // MOBILE_CAM diff --git a/sw/airborne/modules/cam_control/point.c b/sw/airborne/modules/cam_control/point.c index 5be42ec953..2e74bdec70 100644 --- a/sw/airborne/modules/cam_control/point.c +++ b/sw/airborne/modules/cam_control/point.c @@ -344,7 +344,7 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude, vMultiplyMatrixByVector(&sv_cam_projection, smRotation, sv_cam_projection_buf); #if defined(RADIO_CAM_LOCK) - float radio_cam_lock = imcu_get_radio(RADIO_CAM_LOCK); + float radio_cam_lock = radio_control_get(RADIO_CAM_LOCK); if ((radio_cam_lock > MAX_PPRZ / 2) && autopilot_get_mode() == AP_MODE_AUTO2) { cam_lock = true; } if ((radio_cam_lock < MIN_PPRZ / 2) && autopilot_get_mode() == AP_MODE_AUTO2) { cam_lock = false; } #endif diff --git a/sw/airborne/modules/com/generic_com.c b/sw/airborne/modules/com/generic_com.c index 622695d9a1..4e876672b6 100644 --- a/sw/airborne/modules/com/generic_com.c +++ b/sw/airborne/modules/com/generic_com.c @@ -31,7 +31,7 @@ #include "modules/gps/gps.h" #include "modules/energy/electrical.h" #include "generated/airframe.h" -#include "modules/intermcu/inter_mcu.h" +#include "modules/core/commands.h" #include "autopilot.h" #include "modules/nav/common_nav.h" @@ -83,7 +83,7 @@ void generic_com_periodic(void) uint8_t charge = Min(electrical.vsupply * 10.f, 255); // deciAh com_trans.buf[17] = vsupply; com_trans.buf[18] = charge; - com_trans.buf[19] = (uint8_t)(imcu_get_command(COMMAND_THROTTLE) * 100 / MAX_PPRZ); + com_trans.buf[19] = (uint8_t)(command_get(COMMAND_THROTTLE) * 100 / MAX_PPRZ); com_trans.buf[20] = autopilot_get_mode(); com_trans.buf[21] = nav_block; FillBufWith16bit(com_trans.buf, 22, autopilot.flight_time); diff --git a/sw/airborne/modules/core/abi_common.h b/sw/airborne/modules/core/abi_common.h index 8ab09ba039..afa7ffc964 100644 --- a/sw/airborne/modules/core/abi_common.h +++ b/sw/airborne/modules/core/abi_common.h @@ -33,6 +33,7 @@ #include "math/pprz_algebra_int.h" #include "math/pprz_algebra_float.h" #include "modules/gps/gps.h" +#include "modules/radio_control/radio_control.h" /* Include here headers with structure definition you may want to use with ABI * Ex: '#include "modules/gps/gps.h"' in order to use the GpsState structure */ diff --git a/sw/airborne/modules/core/abi_sender_ids.h b/sw/airborne/modules/core/abi_sender_ids.h index 16b8dcb065..ca84d12db5 100644 --- a/sw/airborne/modules/core/abi_sender_ids.h +++ b/sw/airborne/modules/core/abi_sender_ids.h @@ -538,4 +538,43 @@ #define CLOUD_SENSOR_ID 1 #endif +/* + * RADIO_CONTROL message + */ +#ifndef RADIO_CONTROL_PPM_ID +#define RADIO_CONTROL_PPM_ID 1 +#endif + +#ifndef RADIO_CONTROL_SBUS_ID +#define RADIO_CONTROL_SBUS_ID 2 +#endif + +#ifndef RADIO_CONTROL_SBUS_DUAL_ID +#define RADIO_CONTROL_SBUS_DUAL_ID 3 +#endif + +#ifndef RADIO_CONTROL_SPEKTRUM_ID +#define RADIO_CONTROL_SPEKTRUM_ID 4 +#endif + +#ifndef RADIO_CONTROL_SUPERBITRF_RC_ID +#define RADIO_CONTROL_SUPERBITRF_RC_ID 5 +#endif + +#ifndef RADIO_CONTROL_HOTT_ID +#define RADIO_CONTROL_HOTT_ID 6 +#endif + +#ifndef RADIO_CONTROL_DATALINK_ID +#define RADIO_CONTROL_DATALINK_ID 7 +#endif + +#ifndef RADIO_CONTROL_FRSKY_ID +#define RADIO_CONTROL_FRSKY_ID 8 +#endif + +#ifndef RADIO_CONTROL_INTERMCU_ID +#define RADIO_CONTROL_INTERMCU_ID 9 +#endif + #endif /* ABI_SENDER_IDS_H */ diff --git a/sw/airborne/modules/core/commands.c b/sw/airborne/modules/core/commands.c index 9f7a8c82bc..855100c0c4 100644 --- a/sw/airborne/modules/core/commands.c +++ b/sw/airborne/modules/core/commands.c @@ -1,5 +1,6 @@ /* - * (c) 2006 Pascal Brisset, Antoine Drouin + * Copyright (c) 2006 Pascal Brisset, Antoine Drouin + * Copyright (C) 2021 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,9 +15,8 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ @@ -29,3 +29,38 @@ pprz_t commands[COMMANDS_NB]; const pprz_t commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE; + +pprz_t command_roll_trim = 0; +pprz_t command_pitch_trim = 0; +pprz_t command_yaw_trim = 0; + +#if PERIODIC_TELEMETRY +#include "modules/datalink/telemetry.h" + +static void send_commands(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_COMMANDS(trans, dev, AC_ID , COMMANDS_NB, commands); +} +#endif + +void commands_init(void) +{ + SetCommands(commands_failsafe); + + +#ifdef COMMAND_ROLL_TRIM + command_roll_trim = COMMAND_ROLL_TRIM; +#endif +#ifdef COMMAND_PITCH_TRIM + command_pitch_trim = COMMAND_PITCH_TRIM; +#endif +#ifdef COMMAND_YAW_TRIM + command_yaw_trim = COMMAND_YAW_TRIM; +#endif + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands); +#endif +} + + diff --git a/sw/airborne/modules/core/commands.h b/sw/airborne/modules/core/commands.h index 80838a6985..dd77b96441 100644 --- a/sw/airborne/modules/core/commands.h +++ b/sw/airborne/modules/core/commands.h @@ -1,5 +1,6 @@ /* - * (c) 2006 Pascal Brisset, Antoine Drouin + * Copyright (c) 2006 Pascal Brisset, Antoine Drouin + * Copyright (C) 2021 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,9 +15,8 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ @@ -38,9 +38,42 @@ extern pprz_t commands[COMMANDS_NB]; extern const pprz_t commands_failsafe[COMMANDS_NB]; +/** Settings to trim roll, pitch and yaw commands if defined + */ +extern pprz_t command_roll_trim; +extern pprz_t command_pitch_trim; +extern pprz_t command_yaw_trim; + +// Set all commands from array #define SetCommands(t) { \ int i; \ for(i = 0; i < COMMANDS_NB; i++) commands[i] = t[i]; \ } +/** Set a command value + * @param idx command index + * @param value new value + */ +static inline void command_set(uint8_t idx, pprz_t value) +{ + if (idx < COMMANDS_NB) { + // Bound value ??? + commands[idx] = value; + } +} + +/** Get a command value + * @param idx command index + * @return current value, 0 if index is invalid + */ +static inline pprz_t command_get(uint8_t idx) +{ + if (idx < COMMANDS_NB) { + return commands[idx]; + } + return 0; // is it the best value ??? +} + +extern void commands_init(void); + #endif /* COMMANDS_H */ diff --git a/sw/airborne/modules/core/rc_settings.c b/sw/airborne/modules/core/rc_settings.c index b2ce471394..f441f03c6d 100644 --- a/sw/airborne/modules/core/rc_settings.c +++ b/sw/airborne/modules/core/rc_settings.c @@ -27,7 +27,6 @@ #include "modules/core/rc_settings.h" #include "autopilot.h" #include "firmwares/fixedwing/nav.h" -#include "modules/intermcu/inter_mcu.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" uint8_t rc_settings_mode = 0; @@ -39,7 +38,7 @@ float slider_1_val, slider_2_val; #define ParamValFloat(param_init_val, param_travel, cur_pulse, init_pulse) \ (param_init_val + ((float)(cur_pulse - init_pulse)) * param_travel / (float)MAX_PPRZ) -#define RcChannel(x) (imcu_get_radio(x)) +#define RcChannel(x) (radio_control_get(x)) /** Includes generated code from tuning_rc.xml */ #include "generated/settings.h" diff --git a/sw/airborne/modules/datalink/datalink.c b/sw/airborne/modules/datalink/datalink.c index aca6b2c450..cd196289cb 100644 --- a/sw/airborne/modules/datalink/datalink.c +++ b/sw/airborne/modules/datalink/datalink.c @@ -52,28 +52,23 @@ bool datalink_enabled = true; #endif +void datalink_periodic(void) +{ + datalink_time++; // called at 1Hz +} void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf) { - uint8_t sender_id = SenderIdOfPprzMsg(buf); - uint8_t msg_id = IdOfPprzMsg(buf); + uint8_t msg_id = pprzlink_get_msg_id(buf); + uint8_t class_id = pprzlink_get_msg_class_id(buf); + uint8_t sender_id = pprzlink_get_msg_sender_id(buf); - /* parse telemetry messages coming from other AC */ - if (sender_id != 0) { + // Check that the message is a datalink message + if (class_id == DL_datalink_CLASS_ID) { + /* parse datalink messages coming from ground station */ switch (msg_id) { - default: { - break; - } - } - } else { -#if PPRZLINK_DEFAULT_VER == 2 - // Check that the message is really a datalink message - if (pprzlink_get_msg_class_id(buf) == DL_datalink_CLASS_ID) { -#endif - /* parse datalink messages coming from ground station */ - switch (msg_id) { - case DL_PING: { -#if PPRZLINK_DEFAULT_VER == 2 + case DL_PING: + { // Reply to the sender of the message struct pprzlink_msg msg; msg.trans = trans; @@ -82,18 +77,16 @@ void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t * msg.receiver_id = sender_id; msg.component_id = 0; pprzlink_msg_send_PONG(&msg); -#else - pprz_msg_send_PONG(trans, dev, AC_ID); -#endif } break; - case DL_SETTING : { +#ifndef INTERMCU_FBW + case DL_SETTING : + { if (DL_SETTING_ac_id(buf) != AC_ID) { break; } uint8_t i = DL_SETTING_index(buf); float var = DL_SETTING_value(buf); DlSetting(i, var); -#if PPRZLINK_DEFAULT_VER == 2 // Reply to the sender of the message struct pprzlink_msg msg; msg.trans = trans; @@ -102,17 +95,14 @@ void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t * msg.receiver_id = sender_id; msg.component_id = 0; pprzlink_msg_send_DL_VALUE(&msg, &i, &var); -#else - pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &var); -#endif } break; - case DL_GET_SETTING : { + case DL_GET_SETTING : + { if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; } uint8_t i = DL_GET_SETTING_index(buf); float val = settings_get_value(i); -#if PPRZLINK_DEFAULT_VER == 2 // Reply to the sender of the message struct pprzlink_msg msg; msg.trans = trans; @@ -121,72 +111,69 @@ void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t * msg.receiver_id = sender_id; msg.component_id = 0; pprzlink_msg_send_DL_VALUE(&msg, &i, &val); -#else - pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &val); -#endif } break; +#endif #ifdef RADIO_CONTROL_TYPE_DATALINK - case DL_RC_3CH : + case DL_RC_3CH : #ifdef RADIO_CONTROL_DATALINK_LED - LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); + LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); #endif - parse_rc_3ch_datalink( + parse_rc_3ch_datalink( DL_RC_3CH_throttle_mode(buf), DL_RC_3CH_roll(buf), DL_RC_3CH_pitch(buf)); - break; - case DL_RC_4CH : - if (DL_RC_4CH_ac_id(buf) == AC_ID) { + break; + case DL_RC_4CH : + if (DL_RC_4CH_ac_id(buf) == AC_ID) { #ifdef RADIO_CONTROL_DATALINK_LED - LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); + LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); #endif - parse_rc_4ch_datalink(DL_RC_4CH_mode(buf), - DL_RC_4CH_throttle(buf), - DL_RC_4CH_roll(buf), - DL_RC_4CH_pitch(buf), - DL_RC_4CH_yaw(buf)); - } - break; + parse_rc_4ch_datalink(DL_RC_4CH_mode(buf), + DL_RC_4CH_throttle(buf), + DL_RC_4CH_roll(buf), + DL_RC_4CH_pitch(buf), + DL_RC_4CH_yaw(buf)); + } + break; #endif // RADIO_CONTROL_TYPE_DATALINK #if USE_GPS - case DL_GPS_INJECT : { + case DL_GPS_INJECT : + { // Check if the GPS is for this AC if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { break; } // GPS parse data gps_inject_data( - DL_GPS_INJECT_packet_id(buf), - DL_GPS_INJECT_data_length(buf), - DL_GPS_INJECT_data(buf) - ); + DL_GPS_INJECT_packet_id(buf), + DL_GPS_INJECT_data_length(buf), + DL_GPS_INJECT_data(buf) + ); } break; #if USE_GPS_UBX_RTCM - case DL_RTCM_INJECT : { + case DL_RTCM_INJECT : + { // GPS parse data gps_inject_data(DL_RTCM_INJECT_packet_id(buf), - DL_RTCM_INJECT_data_length(buf), - DL_RTCM_INJECT_data(buf)); + DL_RTCM_INJECT_data_length(buf), + DL_RTCM_INJECT_data(buf)); } break; #endif // USE_GPS_UBX_RTCM #endif // USE_GPS - default: - break; - } -#if PPRZLINK_DEFAULT_VER == 2 + default: + break; } -#endif } /* Parse firmware specific datalink */ firmware_parse_msg(dev, trans, buf); /* Parse modules datalink */ - modules_parse_datalink(msg_id, dev, trans, buf); + modules_parse_datalink(msg_id, class_id, dev, trans, buf); } /* default empty WEAK implementation for firmwares without an extra firmware_parse_msg */ diff --git a/sw/airborne/modules/datalink/datalink.h b/sw/airborne/modules/datalink/datalink.h index daa652936b..7e5ab02140 100644 --- a/sw/airborne/modules/datalink/datalink.h +++ b/sw/airborne/modules/datalink/datalink.h @@ -75,6 +75,9 @@ EXTERN bool datalink_enabled; dl_msg_available = true; \ } +/** periodic function, should be called at 1Hz */ +extern void datalink_periodic(void); + /** Check for new message and parse */ static inline void DlCheckAndParse(struct link_device *dev, struct transport_tx *trans, uint8_t *buf, bool *msg_available, bool update_dl) { diff --git a/sw/airborne/modules/datalink/intermcu_dl.c b/sw/airborne/modules/datalink/intermcu_dl.c new file mode 100644 index 0000000000..a0d03681a0 --- /dev/null +++ b/sw/airborne/modules/datalink/intermcu_dl.c @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2016 Freek van Tienen + * Copyright (C) 2022 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + * + */ + +/** @file modules/datalink/intermcu_dl.c + * @brief datalink forwarder for InterMCU + */ + +#include "modules/datalink/intermcu_dl.h" +#include "modules/datalink/datalink.h" +#include "modules/intermcu/intermcu.h" +#include "pprzlink/intermcu_msg.h" +#include "modules/datalink/telemetry.h" + +#ifndef INTERMCU_DL_UPDATE_DL +#define INTERMCU_DL_UPDATE_DL TRUE +#endif + +struct intermcu_dl_t intermcu_dl; + +/* initialization */ +void intermcu_dl_init(void) +{ +#ifdef TELEMETRY_INTERMCU_DEV + intermcu_dl.dev = &(TELEMETRY_INTERMCU_DEV).device; +#ifdef DOWNLINK_TRANSPORT + intermcu_dl.trans = &(DOWNLINK_TRANSPORT).trans_tx; +#else // DEV defined but not transport +#error "TELEMETRY_INTERMCU_DEV is defined but not DOWNLINK_TRANSPORT" +#endif +#else // no DEV + intermcu_dl.dev = NULL; + intermcu_dl.trans = NULL; +#endif +} + +/** Repack message with same header and send on selected link if possible + */ +void intermcu_dl_repack(struct transport_tx *trans, struct link_device *dev, uint8_t *msg, uint8_t size) +{ + struct pprzlink_msg pmsg; + pmsg.trans = trans; + pmsg.dev = dev; + pmsg.sender_id = pprzlink_get_msg_sender_id(msg); + pmsg.receiver_id = pprzlink_get_msg_receiver_id(msg); + pmsg.component_id = pprzlink_get_msg_component_id(msg); + + if (trans->check_available_space(&pmsg, _FD_ADDR, size)) { + trans->count_bytes(&pmsg, size); + trans->start_message(&pmsg, _FD, size); + trans->put_bytes(&pmsg, _FD, DL_TYPE_UINT8, DL_FORMAT_ARRAY, (void *) msg, size); + trans->end_message(&pmsg, _FD); + } else { + trans->overrun(&pmsg); + } +} + +/** + * function to forward telemetry from AP to the ground + */ +void intermcu_dl_on_msg(uint8_t* msg, uint8_t size) +{ + if (intermcu_dl.dev != NULL) { + intermcu_dl_repack(intermcu_dl.trans, intermcu_dl.dev, msg, size); + } +} + + diff --git a/sw/airborne/modules/datalink/intermcu_dl.h b/sw/airborne/modules/datalink/intermcu_dl.h new file mode 100644 index 0000000000..ab85846a2a --- /dev/null +++ b/sw/airborne/modules/datalink/intermcu_dl.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2022 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + * + */ + +/** @file modules/datalink/intermcu_dl.h + * @brief datalink forwarder for InterMCU + */ + +#ifndef INTERMCU_DL_H +#define INTERMCU_DL_H + +#include "std.h" +#include "pprzlink/pprzlink_device.h" +#include "pprzlink/pprzlink_transport.h" + +/* Structure for forwarding telemetry over InterMCU */ +struct intermcu_dl_t { + struct link_device *dev; ///< Device structure for communication + struct transport_tx *trans; ///< Forwarding transport +}; + +extern struct intermcu_dl_t intermcu_dl; + +/** + * Init function + */ +extern void intermcu_dl_init(void); + +/** repack a message and send on device + * + * @param trans pointer to the TX transport + * @param dev pointer to the sending device + * @param msg pointer to the message buffer + * @param size size of the message to send + */ +extern void intermcu_dl_repack(struct transport_tx *trans, struct link_device *dev, uint8_t *msg, uint8_t size); + +/** function to forward telemetry on new message + * + * @param msg pointer to the message buffer + * @param size size of the message to send + */ +extern void intermcu_dl_on_msg(uint8_t* msg, uint8_t size); + +#endif /* INTERMCU_DL_H */ + + diff --git a/sw/airborne/modules/datalink/telemetry.c b/sw/airborne/modules/datalink/telemetry.c index d585ff6020..65c8a934f2 100644 --- a/sw/airborne/modules/datalink/telemetry.c +++ b/sw/airborne/modules/datalink/telemetry.c @@ -28,7 +28,10 @@ */ #include "modules/datalink/telemetry_common.h" +#include "modules/datalink/telemetry.h" #include "generated/periodic_telemetry.h" +#include "autopilot.h" + /* Implement global structures from generated header. * Can register up to #TELEMETRY_NB_CBS callbacks per periodic message. @@ -66,6 +69,36 @@ int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, return -1; } +/** Peridioc task + * Send a series of initialisation messages followed by a stream of periodic ones. + */ +void telemetry_reporting_task(void) +{ + static uint8_t boot = true; + + /* initialisation phase during boot */ + if (boot) { +#if DOWNLINK && !(defined INTERMCU_FBW) + autopilot_send_version(); +#endif + boot = false; + } + /* then report periodicly */ + else { +#if FIXEDWING_FIRMWARE +#if AP + periodic_telemetry_send_Ap(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); +#endif // AP +#if FBW || !TELEMETRY_INTERMCU // FIXME for now send both AP and FBW process if needed + periodic_telemetry_send_Fbw(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); +#endif //FBW +#else // ROTORCRAFT && ROVER + periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); +#endif + } +} + + #if USE_PERIODIC_TELEMETRY_REPORT #include "modules/datalink/downlink.h" diff --git a/sw/airborne/modules/datalink/telemetry_common.h b/sw/airborne/modules/datalink/telemetry_common.h index 0f5eda01c3..30aa513505 100644 --- a/sw/airborne/modules/datalink/telemetry_common.h +++ b/sw/airborne/modules/datalink/telemetry_common.h @@ -64,6 +64,10 @@ struct periodic_telemetry { struct telemetry_cb_slots *cbs; ///< array of callbacks defined through TELEMETRY_MSG }; +/** Periodic task + */ +extern void telemetry_reporting_task(void); + /** Register a telemetry callback function. * empty implementation is provided if PERIODIC_TELEMETRY is not set or set to FALSE * @param _pt periodic telemetry structure to register diff --git a/sw/airborne/modules/digital_cam/dc_shoot_rc.c b/sw/airborne/modules/digital_cam/dc_shoot_rc.c index a6236399da..7d683d8aac 100644 --- a/sw/airborne/modules/digital_cam/dc_shoot_rc.c +++ b/sw/airborne/modules/digital_cam/dc_shoot_rc.c @@ -27,7 +27,7 @@ */ #include "dc_shoot_rc.h" -#include "modules/intermcu/inter_mcu.h" +#include "modules/radio_control/radio_control.h" #include "dc.h" #ifndef DC_RADIO_SHOOT @@ -41,7 +41,7 @@ void dc_shoot_rc_periodic(void) static uint8_t rd_shoot = 0; static uint8_t rd_num = 0; - if ((rd_shoot == 0) && (imcu_get_radio(DC_RADIO_SHOOT) > DC_RADIO_SHOOT_THRESHOLD)) { + if ((rd_shoot == 0) && (radio_control_get(DC_RADIO_SHOOT) > DC_RADIO_SHOOT_THRESHOLD)) { dc_send_command(DC_SHOOT); rd_shoot = 1; } diff --git a/sw/airborne/modules/digital_cam/servo_cam_ctrl.c b/sw/airborne/modules/digital_cam/servo_cam_ctrl.c index 320f1ed61a..a2268d833f 100644 --- a/sw/airborne/modules/digital_cam/servo_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/servo_cam_ctrl.c @@ -33,11 +33,11 @@ // Include Servo and airframe servo channels #include "std.h" -#include "modules/intermcu/inter_mcu.h" +#include "modules/core/commands.h" #include "generated/airframe.h" -#define DC_PUSH(X) imcu_set_command(X, -MAX_PPRZ); -#define DC_RELEASE(X) imcu_set_command(X, MAX_PPRZ); +#define DC_PUSH(X) command_set(X, -MAX_PPRZ); +#define DC_RELEASE(X) command_set(X, MAX_PPRZ); /** how long to push shutter in seconds */ #ifndef DC_SHUTTER_DELAY diff --git a/sw/airborne/modules/display/max7456.c b/sw/airborne/modules/display/max7456.c index 7a87b97076..efb97274c4 100644 --- a/sw/airborne/modules/display/max7456.c +++ b/sw/airborne/modules/display/max7456.c @@ -28,7 +28,7 @@ #include "std.h" //#include "stdio.h" -#include "modules/intermcu/inter_mcu.h" +#include "modules/core/commands.h" #include "mcu_periph/sys_time.h" #include "mcu_periph/gpio.h" @@ -776,7 +776,7 @@ void draw_osd(void) case (50): #if defined(FIXEDWING_FIRMWARE) - osd_sprintf(osd_string, "THR%.0f", (((float)ap_state->commands[COMMAND_THROTTLE] / (float)MAX_PPRZ) * 100.)); + osd_sprintf(osd_string, "THR%.0f", (((float)command_get(COMMAND_THROTTLE) / (float)MAX_PPRZ) * 100.)); #else osd_sprintf(osd_string, "THR%.0fTHR", (((float)stabilization_cmd[COMMAND_THRUST] / (float)MAX_PPRZ) * 100.)); #endif diff --git a/sw/airborne/modules/gas_engine/gas_engine_idle_trim.c b/sw/airborne/modules/gas_engine/gas_engine_idle_trim.c index 38c4eb16ed..0ceef9688c 100644 --- a/sw/airborne/modules/gas_engine/gas_engine_idle_trim.c +++ b/sw/airborne/modules/gas_engine/gas_engine_idle_trim.c @@ -25,12 +25,12 @@ int gas_engine_idle_trim_left = 0; int gas_engine_idle_trim_right = 0; -#include "modules/intermcu/inter_mcu.h" +#include "modules/core/commands.h" void periodic_gas_engine_idle_trim(void) { - imcu_set_command(COMMAND_IDLE1, imcu_get_radio(RADIO_GAIN1)); - imcu_set_command(COMMAND_IDLE2, imcu_get_radio(RADIO_GAIN2)); + command_set(COMMAND_IDLE1, radio_control_get(RADIO_GAIN1)); + command_set(COMMAND_IDLE2, radio_control_get(RADIO_GAIN2)); } diff --git a/sw/airborne/modules/gps/gps_intermcu.c b/sw/airborne/modules/gps/gps_intermcu.c new file mode 100644 index 0000000000..cc27e835b4 --- /dev/null +++ b/sw/airborne/modules/gps/gps_intermcu.c @@ -0,0 +1,80 @@ +/* + * Copyright (C) 2022 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/gps/gps_intermcu.c + * @brief GPS system based on intermcu + */ + +#include "modules/gps/gps_intermcu.h" +#include "modules/core/abi.h" +#include "pprzlink/intermcu_msg.h" + +struct GpsState gps_imcu; + +/** GPS initialization */ +void gps_intermcu_init(void) +{ + gps_imcu.fix = GPS_FIX_NONE; + gps_imcu.pdop = 0; + gps_imcu.sacc = 0; + gps_imcu.pacc = 0; + gps_imcu.cacc = 0; + gps_imcu.comp_id = GPS_IMCU_ID; +} + +void gps_intermcu_parse_IMCU_REMOTE_GPS(uint8_t *buf) +{ + uint32_t now_ts = get_sys_time_usec(); + + gps_imcu.ecef_pos.x = DL_IMCU_REMOTE_GPS_ecef_x(buf); + gps_imcu.ecef_pos.y = DL_IMCU_REMOTE_GPS_ecef_y(buf); + gps_imcu.ecef_pos.z = DL_IMCU_REMOTE_GPS_ecef_z(buf); + SetBit(gps_imcu.valid_fields, GPS_VALID_POS_ECEF_BIT); + + gps_imcu.lla_pos.alt = DL_IMCU_REMOTE_GPS_alt(buf); + gps_imcu.hmsl = DL_IMCU_REMOTE_GPS_hmsl(buf); + SetBit(gps_imcu.valid_fields, GPS_VALID_HMSL_BIT); + + gps_imcu.ecef_vel.x = DL_IMCU_REMOTE_GPS_ecef_xd(buf); + gps_imcu.ecef_vel.y = DL_IMCU_REMOTE_GPS_ecef_yd(buf); + gps_imcu.ecef_vel.z = DL_IMCU_REMOTE_GPS_ecef_zd(buf); + SetBit(gps_imcu.valid_fields, GPS_VALID_VEL_ECEF_BIT); + + gps_imcu.course = DL_IMCU_REMOTE_GPS_course(buf); + gps_imcu.gspeed = DL_IMCU_REMOTE_GPS_gspeed(buf); + SetBit(gps_imcu.valid_fields, GPS_VALID_COURSE_BIT); + + gps_imcu.pacc = DL_IMCU_REMOTE_GPS_pacc(buf); + gps_imcu.sacc = DL_IMCU_REMOTE_GPS_sacc(buf); + gps_imcu.num_sv = DL_IMCU_REMOTE_GPS_numsv(buf); + gps_imcu.fix = DL_IMCU_REMOTE_GPS_fix(buf); + + // set gps msg time + gps_imcu.last_msg_ticks = sys_time.nb_sec_rem; + gps_imcu.last_msg_time = sys_time.nb_sec; + if (gps_imcu.fix >= GPS_FIX_3D) { + gps_imcu.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_imcu.last_3dfix_time = sys_time.nb_sec; + } + + AbiSendMsgGPS(GPS_IMCU_ID, now_ts, &gps_imcu); +} + diff --git a/sw/airborne/firmwares/rover/main_ap.h b/sw/airborne/modules/gps/gps_intermcu.h similarity index 56% rename from sw/airborne/firmwares/rover/main_ap.h rename to sw/airborne/modules/gps/gps_intermcu.h index c8c58c8522..1198ae5c29 100644 --- a/sw/airborne/firmwares/rover/main_ap.h +++ b/sw/airborne/modules/gps/gps_intermcu.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Gautier Hattenberger + * Copyright (C) 2022 Gautier Hattenberger * * This file is part of paparazzi. * @@ -19,22 +19,27 @@ */ /** - * @file firmwares/rover/main_ap.h - * - * Rover main loop. + * @file modules/gps/gps_intermcu.h + * @brief GPS system based on intermcu */ -#ifndef MAIN_H -#define MAIN_H +#ifndef GPS_INTERMCU_H +#define GPS_INTERMCU_H -extern void main_init(void); -extern void main_event(void); -extern void handle_periodic_tasks(void); +#include "std.h" +#include "generated/airframe.h" +#include "modules/gps/gps.h" -extern void main_periodic(void); -extern void telemetry_periodic(void); -extern void failsafe_check(void); +#ifndef PRIMARY_GPS +#define PRIMARY_GPS GPS_INTERMCU +#endif +extern struct GpsState gps_imcu; -#endif /* MAIN_H */ +extern void gps_intermcu_init(void); +#define gps_intermcu_periodic_check() gps_periodic_check(&gps_imcu) + +extern void gps_intermcu_parse_IMCU_REMOTE_GPS(uint8_t *buf); + +#endif /* GPS_INTERMCU_H */ diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c index f463b51bc2..dddcdfac67 100644 --- a/sw/airborne/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/modules/ins/ins_arduimu_basic.c @@ -35,7 +35,7 @@ // Command vector for thrust #include "generated/airframe.h" -#include "modules/intermcu/inter_mcu.h" +#include "modules/core/commands.h" #define NB_DATA 9 @@ -112,7 +112,7 @@ void ArduIMU_periodicGPS(void) // - low speed // - high thrust float speed = stateGetHorizontalSpeedNorm_f(); - pprz_t cmd = imcu_get_command(COMMAND_THROTTLE); + pprz_t cmd = command_get(COMMAND_THROTTLE); if (speed < HIGH_ACCEL_LOW_SPEED && cmd > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) { high_accel_flag = true; } else { diff --git a/sw/airborne/modules/intermcu/inter_mcu.c b/sw/airborne/modules/intermcu/inter_mcu.c deleted file mode 100644 index 955905f578..0000000000 --- a/sw/airborne/modules/intermcu/inter_mcu.c +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Copyright (C) 2003-2005 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - */ - -/** - * @file modules/intermcu/inter_mcu.c - * Communication between fbw and ap processes. - * - */ - -#include "modules/intermcu/inter_mcu.h" - -#if defined SINGLE_MCU -static struct fbw_state _fbw_state; -static struct ap_state _ap_state; -struct fbw_state *fbw_state = &_fbw_state; -struct ap_state *ap_state = &_ap_state; -#else /* SINGLE_MCU */ -#include "modules/intermcu/link_mcu.h" -struct fbw_state *fbw_state = &link_mcu_from_fbw_msg.payload.from_fbw; -struct ap_state *ap_state = &link_mcu_from_ap_msg.payload.from_ap; -#endif /* ! SINGLE_MCU */ - -volatile bool inter_mcu_received_fbw = false; -volatile bool inter_mcu_received_ap = false; - -PPRZ_MUTEX(ap_state_mtx); -PPRZ_MUTEX(fbw_state_mtx); - -#ifdef FBW -/** Variables for monitoring AP communication status */ -bool ap_ok; -uint8_t time_since_last_ap; -#endif diff --git a/sw/airborne/modules/intermcu/inter_mcu.h b/sw/airborne/modules/intermcu/inter_mcu.h deleted file mode 100644 index e9526ef1ab..0000000000 --- a/sw/airborne/modules/intermcu/inter_mcu.h +++ /dev/null @@ -1,344 +0,0 @@ -/* - * Copyright (C) 2003-2005 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - */ - -/** - * @file modules/intermcu/inter_mcu.h - * Communication between fbw and ap processes. - * - * This unit contains the data structure used to communicate between the - * "fly by wire" process and the "autopilot" process. It must be linked once in a - * monoprocessor architecture, twice in a twin-processors architecture. - * In the latter case, the inter-mcu communication process (e.g. SPI) must fill and - * read these data structures. - */ - -#ifndef INTER_MCU_H -#define INTER_MCU_H - - -#ifdef INTER_MCU - -#include - -#include "std.h" - -#include "paparazzi.h" -#include "pprz_mutex.h" -#include "generated/airframe.h" -#include "modules/radio_control/radio_control.h" -#include "modules/energy/electrical.h" -#include "firmwares/fixedwing/main_fbw.h" - -/** FIXME dummy definition for compat with rotorcraft **/ -static inline void intermcu_init(void) {} -static inline void intermcu_periodic(void) {} - -/** Data structure shared by fbw and ap processes */ -struct fbw_state { -#if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1 - pprz_t channels[RADIO_CONTROL_NB_CHANNEL]; - uint8_t ppm_cpt; -#endif - uint8_t status; - uint8_t nb_err; - struct Electrical electrical; -}; - -struct ap_state { - pprz_t commands[COMMANDS_NB]; - pprz_t command_roll_trim; - pprz_t command_pitch_trim; - pprz_t command_yaw_trim; -}; - -// Status bits from FBW to AUTOPILOT -#define STATUS_RADIO_OK 0 -#define STATUS_RADIO_REALLY_LOST 1 -#define STATUS_MODE_AUTO 2 -#define STATUS_MODE_FAILSAFE 3 -#define AVERAGED_CHANNELS_SENT 4 -#define MASK_FBW_CHANGED 0xf - - -extern struct fbw_state *fbw_state; -extern struct ap_state *ap_state; - -extern volatile bool inter_mcu_received_fbw; -extern volatile bool inter_mcu_received_ap; - -/** - * Getter and setter functions for ap_state and fbw_state elements - * protected by mutexes - */ -PPRZ_MUTEX_DECL(ap_state_mtx); -PPRZ_MUTEX_DECL(fbw_state_mtx); - -/** get AP command - * @param cmd_idx command index - * @return command value - */ -static inline pprz_t imcu_get_command(uint8_t cmd_idx) -{ - PPRZ_MUTEX_LOCK(ap_state_mtx); - pprz_t val = ap_state->commands[cmd_idx]; - PPRZ_MUTEX_UNLOCK(ap_state_mtx); - return val; -} - -/** set AP command - * @param cmd_idx command index - * @param cmd new command value - */ -static inline void imcu_set_command(uint8_t cmd_idx, pprz_t cmd) -{ - PPRZ_MUTEX_LOCK(ap_state_mtx); - ap_state->commands[cmd_idx] = cmd; - PPRZ_MUTEX_UNLOCK(ap_state_mtx); -} - -/** get roll trim - * @return roll trim value - */ -static inline pprz_t imcu_get_roll_trim(void) -{ - PPRZ_MUTEX_LOCK(ap_state_mtx); - pprz_t val = ap_state->command_roll_trim; - PPRZ_MUTEX_UNLOCK(ap_state_mtx); - return val; -} - -/** set roll trim - * @param roll_trim new roll trim value - */ -static inline void imcu_set_roll_trim(pprz_t roll_trim) -{ - PPRZ_MUTEX_LOCK(ap_state_mtx); - ap_state->command_roll_trim = roll_trim; - PPRZ_MUTEX_UNLOCK(ap_state_mtx); -} - -/** get pitch trim - * @return pitch trim value - */ -static inline pprz_t imcu_get_pitch_trim(void) -{ - PPRZ_MUTEX_LOCK(ap_state_mtx); - pprz_t val = ap_state->command_pitch_trim; - PPRZ_MUTEX_UNLOCK(ap_state_mtx); - return val; -} - -/** set pitch trim - * @param pitch_trim new pitch trim value - */ -static inline void imcu_set_pitch_trim(pprz_t pitch_trim) -{ - PPRZ_MUTEX_LOCK(ap_state_mtx); - ap_state->command_pitch_trim = pitch_trim; - PPRZ_MUTEX_UNLOCK(ap_state_mtx); -} - -/** get yaw trim - * @return yaw trim value - */ -static inline pprz_t imcu_get_yaw_trim(void) -{ - PPRZ_MUTEX_LOCK(ap_state_mtx); - pprz_t val = ap_state->command_yaw_trim; - PPRZ_MUTEX_UNLOCK(ap_state_mtx); - return val; -} - -/** set yaw trim - * @param yaw_trim new yaw trim value - */ -static inline void imcu_set_yaw_trim(pprz_t yaw_trim) -{ - PPRZ_MUTEX_LOCK(ap_state_mtx); - ap_state->command_yaw_trim = yaw_trim; - PPRZ_MUTEX_UNLOCK(ap_state_mtx); -} - -#if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1 -/** get radio channel value - * @param radio_idx radio channel index - * @return radio channel value - */ -static inline pprz_t imcu_get_radio(uint8_t radio_idx) -{ - PPRZ_MUTEX_LOCK(fbw_state_mtx); - pprz_t val = fbw_state->channels[radio_idx]; - PPRZ_MUTEX_UNLOCK(fbw_state_mtx); - return val; -} - -/** set radio channel - * @param radio_idx radio index - * @param radio new radio channel value - */ -static inline void imcu_set_radio(uint8_t radio_idx, pprz_t radio) -{ - PPRZ_MUTEX_LOCK(fbw_state_mtx); - fbw_state->channels[radio_idx] = radio; - PPRZ_MUTEX_UNLOCK(fbw_state_mtx); -} - -/** get ppm count value - * @return ppm count - */ -static inline uint8_t imcu_get_ppm_cpt(void) -{ - PPRZ_MUTEX_LOCK(fbw_state_mtx); - uint8_t val = fbw_state->ppm_cpt; - PPRZ_MUTEX_UNLOCK(fbw_state_mtx); - return val; -} - -/** set ppm count - * @param ppm_cpt ppm counter - */ -static inline void imcu_set_ppm_cpt(uint8_t ppm_cpt) -{ - PPRZ_MUTEX_LOCK(fbw_state_mtx); - fbw_state->ppm_cpt = ppm_cpt; - PPRZ_MUTEX_UNLOCK(fbw_state_mtx); -} -#endif - -/** get FBW status - * @return status - */ -static inline uint8_t imcu_get_status(void) -{ - PPRZ_MUTEX_LOCK(fbw_state_mtx); - uint8_t val = fbw_state->status; - PPRZ_MUTEX_UNLOCK(fbw_state_mtx); - return val; -} - -/** set FBW status - * @param status FBW status - */ -static inline void imcu_set_status(uint8_t status) -{ - PPRZ_MUTEX_LOCK(fbw_state_mtx); - fbw_state->status = status; - PPRZ_MUTEX_UNLOCK(fbw_state_mtx); -} - -/** get electrical parameters - * @param _electrical pointer to electrical data to fill - */ -static inline void imcu_get_electrical(struct Electrical *_electrical) -{ - PPRZ_MUTEX_LOCK(fbw_state_mtx); - *_electrical = fbw_state->electrical; - PPRZ_MUTEX_UNLOCK(fbw_state_mtx); -} - -/** set electrical parameters - * @param _electrical pointer to electrical data to use - */ -static inline void imcu_set_electrical(struct Electrical *_electrical) -{ - PPRZ_MUTEX_LOCK(fbw_state_mtx); - fbw_state->electrical = *_electrical; - PPRZ_MUTEX_UNLOCK(fbw_state_mtx); -} - -#ifdef FBW - -extern uint8_t time_since_last_ap; -extern bool ap_ok; - -#define AP_STALLED_TIME 30 // 500ms with a 60Hz timer - - -static inline void inter_mcu_init(void) -{ - fbw_state->status = 0; - fbw_state->nb_err = 0; - - PPRZ_MUTEX_INIT(ap_state_mtx); - PPRZ_MUTEX_INIT(fbw_state_mtx); - - ap_ok = false; -} - - -/* Prepare data to be sent to mcu0 */ -static inline void inter_mcu_fill_fbw_state(void) -{ - PPRZ_MUTEX_LOCK(fbw_state_mtx); - uint8_t status = 0; - -#ifdef RADIO_CONTROL - uint8_t i; - for (i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) { - fbw_state->channels[i] = radio_control.values[i]; - } - - fbw_state->ppm_cpt = radio_control.frame_rate; - - status = (radio_control.status == RC_OK ? _BV(STATUS_RADIO_OK) : 0); - status |= (radio_control.status == RC_REALLY_LOST ? _BV(STATUS_RADIO_REALLY_LOST) : 0); - status |= (radio_control.status == RC_OK ? _BV(AVERAGED_CHANNELS_SENT) : - 0); // Any valid frame contains averaged channels -#endif // RADIO_CONTROL - - status |= (fbw_mode == FBW_MODE_AUTO ? _BV(STATUS_MODE_AUTO) : 0); - status |= (fbw_mode == FBW_MODE_FAILSAFE ? _BV(STATUS_MODE_FAILSAFE) : 0); - fbw_state->status = status; - - fbw_state->electrical = electrical; -#if defined SINGLE_MCU - /**Directly set the flag indicating to AP that shared buffer is available*/ - inter_mcu_received_fbw = true; -#endif - PPRZ_MUTEX_UNLOCK(fbw_state_mtx); -} - -/** Prepares date for next comm with AP. Set ::ap_ok to TRUE */ -static inline void inter_mcu_event_task(void) -{ - time_since_last_ap = 0; - ap_ok = true; -} - -/** Monitors AP. Set ::ap_ok to false if AP is down for a long time. */ -static inline void inter_mcu_periodic_task(void) -{ - if (time_since_last_ap >= AP_STALLED_TIME) { - ap_ok = false; -#ifdef SINGLE_MCU - // Keep filling the buffer even if no AP commands are received - inter_mcu_fill_fbw_state(); -#endif - - } else { - time_since_last_ap++; - } -} - -#endif /* FBW */ - -#endif /* INTER_MCU */ - -#endif /* INTER_MCU_H */ diff --git a/sw/airborne/modules/intermcu/intermcu.h b/sw/airborne/modules/intermcu/intermcu.h index 7e53aefc3e..cdc03869d6 100644 --- a/sw/airborne/modules/intermcu/intermcu.h +++ b/sw/airborne/modules/intermcu/intermcu.h @@ -21,19 +21,16 @@ */ /** @file modules/intermcu/intermcu.h - * @brief Rotorcraft Inter-MCU interface + * @brief Inter-MCU interface */ -#ifndef INTERMCU_ROTORCRAFT_H -#define INTERMCU_ROTORCRAFT_H +#ifndef INTERMCU_H +#define INTERMCU_H #include "std.h" #include "modules/core/commands.h" #include "pprzlink/pprz_transport.h" -#define INTERMCU_AP 0 -#define INTERMCU_FBW 1 - #ifndef INTERMCU_LOST_CNT #define INTERMCU_LOST_CNT 25 /* 50ms with a 512Hz timer TODO fixed value */ #endif @@ -46,16 +43,18 @@ enum intermcu_status { INTERMCU_LOST ///< No interMCU communication anymore }; +#ifdef BOARD_PX4IO /* InterMCU baudrate protection for PX4 */ enum intermcu_PX4_baud_status { PX4_BAUD, CHANGING_BAUD, PPRZ_BAUD }; +#endif /* InterMCU command status bits */ enum intermcu_cmd_status { - INTERMCU_CMD_MOTORS_ON, ///< The status of autopilot_motors_on + INTERMCU_CMD_MOTORS_ON, ///< The status of intermcu_ap_motors_on INTERMCU_CMD_DISARM, ///< Whether or not to dis-arm the FBW INTERMCU_CMD_TIPPROPS, ///< Enable tip props INTERMCU_CMD_FAILSAFE, ///< Set FBW in failsafe mode @@ -80,10 +79,13 @@ struct intermcu_t { enum intermcu_PX4_baud_status stable_px4_baud; #endif }; + extern struct intermcu_t intermcu; /* Functions defined in XML */ -void intermcu_init(void); -void intermcu_periodic(void); +extern void intermcu_init(void); +extern void intermcu_periodic(void); +extern void intermcu_event(void); #endif + diff --git a/sw/airborne/modules/intermcu/intermcu_ap.c b/sw/airborne/modules/intermcu/intermcu_ap.c index da272f4931..8cd7362083 100644 --- a/sw/airborne/modules/intermcu/intermcu_ap.c +++ b/sw/airborne/modules/intermcu/intermcu_ap.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2015 Freek van Tienen + * Copyright (C) 2022 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,26 +15,29 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ /** @file modules/intermcu/intermcu_ap.c - * @brief Rotorcraft Inter-MCU on the autopilot + * @brief Inter-MCU on the AP side */ -#include "intermcu_ap.h" -#include "pprzlink/intermcu_msg.h" -#include "modules/radio_control/radio_control.h" -#include "mcu_periph/uart.h" +#define PERIODIC_C_INTERMCU +#include "modules/intermcu/intermcu_ap.h" +#include "pprzlink/intermcu_msg.h" #include "modules/energy/electrical.h" +#include "generated/modules.h" #include "autopilot.h" +#if TELEMETRY_INTERMCU +#include "modules/datalink/intermcu_dl.h" +#endif +#include "generated/periodic_telemetry.h" #if COMMANDS_NB > 8 -#error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED" +#warning "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART MIGHT BE OVERFILLED" #endif @@ -43,16 +47,10 @@ struct intermcu_t intermcu = { .enabled = true, .msg_available = false, }; -uint8_t imcu_msg_buf[128] __attribute__((aligned)); ///< The InterMCU message buffer -static struct fbw_status_t fbw_status; -static inline void intermcu_parse_msg(void (*rc_frame_handler)(void)); -#if IMCU_GPS -#include "std.h" -#include "modules/core/abi.h" -#include "modules/gps/gps.h" -static struct GpsState gps_imcu; -#endif +uint8_t imcu_msg_buf[128] __attribute__((aligned)); ///< The InterMCU message buffer + +static struct fbw_status_t fbw_status; #if PERIODIC_TELEMETRY @@ -63,7 +61,7 @@ static void send_status(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, &fbw_status.rc_status, &fbw_status.frame_rate, &fbw_status.mode, - &fbw_status.electrical.vsupply, &fbw_status.electrical.current); + &electrical.vsupply, &electrical.current); } #endif @@ -72,15 +70,6 @@ void intermcu_init(void) { pprz_transport_init(&intermcu.transport); -#if IMCU_GPS - gps_imcu.fix = GPS_FIX_NONE; - gps_imcu.pdop = 0; - gps_imcu.sacc = 0; - gps_imcu.pacc = 0; - gps_imcu.cacc = 0; - gps_imcu.comp_id = GPS_IMCU_ID; -#endif - #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_status); #endif @@ -96,19 +85,48 @@ void intermcu_periodic(void) intermcu.time_since_last_frame++; } -#if IMCU_GPS - RunOnceEvery(PERIODIC_FREQUENCY, gps_periodic_check(&gps_imcu)); +#ifdef TELEMETRY_PROCESS_InterMCU + // send periodic InterMCU if defined in telemetry file + periodic_telemetry_send_InterMCU(DefaultPeriodic, &intermcu.transport.trans_tx, intermcu.device); #endif } +/* Check new characters */ +void intermcu_event(void) +{ + /* Parse incoming bytes */ + if (intermcu.enabled) { + pprz_check_and_parse(intermcu.device, &intermcu.transport, imcu_msg_buf, &intermcu.msg_available); + if (intermcu.msg_available) { + uint8_t class_id = pprzlink_get_msg_class_id(imcu_msg_buf); + // reset intermcu timer, don't touch msg_available flag + intermcu.time_since_last_frame = 0; + + if (class_id == DL_intermcu_CLASS_ID) { + // parse intermcu messages and call callbacks + dl_parse_msg(intermcu.device, &intermcu.transport.trans_tx, imcu_msg_buf); + } else { + // reset datalink_time if message is not intermcu class + datalink_time = 0; + datalink_nb_msgs++; +#if TELEMETRY_INTERMCU + // forward all other messages if needed + intermcu_dl_on_msg(imcu_msg_buf, intermcu.transport.trans_rx.payload_len); +#endif + } + intermcu.msg_available = false; + } + } +} + /* Enable or disable the communication of the InterMCU */ void intermcu_set_enabled(bool value) { intermcu.enabled = value; } -/* Send the actuators to the FBW */ -void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute__((unused))) +/* Send the commands to the FBW */ +void intermcu_send_commands(pprz_t *command_values, uint8_t ap_mode __attribute__((unused))) { if (!intermcu.enabled) { return; @@ -121,7 +139,7 @@ void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute_ // Send the message and reset cmd_status pprz_msg_send_IMCU_COMMANDS(&(intermcu.transport.trans_tx), intermcu.device, - INTERMCU_AP, &intermcu.cmd_status, COMMANDS_NB, command_values); //TODO: Append more status + AC_ID, &intermcu.cmd_status, COMMANDS_NB, command_values); //TODO: Append more status intermcu.cmd_status = 0; } @@ -129,114 +147,16 @@ void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute_ void intermcu_send_spektrum_bind(void) { if (intermcu.enabled) { - pprz_msg_send_IMCU_SPEKTRUM_SOFT_BIND(&(intermcu.transport.trans_tx), intermcu.device, INTERMCU_AP); + pprz_msg_send_IMCU_SPEKTRUM_SOFT_BIND(&(intermcu.transport.trans_tx), intermcu.device, AC_ID); } } -/* Parse incomming InterMCU messages */ -#pragma GCC diagnostic ignored "-Wcast-align" -static inline void intermcu_parse_msg(void (*rc_frame_handler)(void)) +void intermcu_parse_IMCU_FBW_STATUS(uint8_t *buf) { - /* Parse the Inter MCU message */ - uint8_t msg_id = pprzlink_get_msg_id(imcu_msg_buf); -#if PPRZLINK_DEFAULT_VER == 2 - // Check that the message is really a intermcu message - if (pprzlink_get_msg_class_id(imcu_msg_buf) == DL_intermcu_CLASS_ID) { -#endif - switch (msg_id) { - case DL_IMCU_RADIO_COMMANDS: { - uint8_t i; - uint8_t size = DL_IMCU_RADIO_COMMANDS_values_length(imcu_msg_buf); - intermcu.status = DL_IMCU_RADIO_COMMANDS_status(imcu_msg_buf); - for (i = 0; i < size; i++) { - radio_control.values[i] = DL_IMCU_RADIO_COMMANDS_values(imcu_msg_buf)[i]; - } - - radio_control.frame_cpt++; - radio_control.time_since_last_frame = 0; - radio_control.status = RC_OK; - rc_frame_handler(); - break; - } - - case DL_IMCU_FBW_STATUS: { - fbw_status.rc_status = DL_IMCU_FBW_STATUS_rc_status(imcu_msg_buf); - fbw_status.frame_rate = DL_IMCU_FBW_STATUS_frame_rate(imcu_msg_buf); - fbw_status.mode = DL_IMCU_FBW_STATUS_mode(imcu_msg_buf); - fbw_status.electrical.vsupply = DL_IMCU_FBW_STATUS_vsupply(imcu_msg_buf); - fbw_status.electrical.current = DL_IMCU_FBW_STATUS_current(imcu_msg_buf); - break; - } - - #if TELEMETRY_INTERMCU - case DL_IMCU_DATALINK: { - uint8_t size = DL_IMCU_DATALINK_msg_length(imcu_msg_buf); - uint8_t *msg = DL_IMCU_DATALINK_msg(imcu_msg_buf); - telemetry_intermcu_on_msg(msg, size); - break; - } - #endif - - #if IMCU_GPS - case DL_IMCU_REMOTE_GPS: { - uint32_t now_ts = get_sys_time_usec(); - gps_imcu.ecef_pos.x = DL_IMCU_REMOTE_GPS_ecef_x(imcu_msg_buf); - gps_imcu.ecef_pos.y = DL_IMCU_REMOTE_GPS_ecef_y(imcu_msg_buf); - gps_imcu.ecef_pos.z = DL_IMCU_REMOTE_GPS_ecef_z(imcu_msg_buf); - SetBit(gps_imcu.valid_fields, GPS_VALID_POS_ECEF_BIT); - - gps_imcu.lla_pos.alt = DL_IMCU_REMOTE_GPS_alt(imcu_msg_buf); - gps_imcu.hmsl = DL_IMCU_REMOTE_GPS_hmsl(imcu_msg_buf); - SetBit(gps_imcu.valid_fields, GPS_VALID_HMSL_BIT); - - gps_imcu.ecef_vel.x = DL_IMCU_REMOTE_GPS_ecef_xd(imcu_msg_buf); - gps_imcu.ecef_vel.y = DL_IMCU_REMOTE_GPS_ecef_yd(imcu_msg_buf); - gps_imcu.ecef_vel.z = DL_IMCU_REMOTE_GPS_ecef_zd(imcu_msg_buf); - SetBit(gps_imcu.valid_fields, GPS_VALID_VEL_ECEF_BIT); - - gps_imcu.course = DL_IMCU_REMOTE_GPS_course(imcu_msg_buf); - gps_imcu.gspeed = DL_IMCU_REMOTE_GPS_gspeed(imcu_msg_buf); - SetBit(gps_imcu.valid_fields, GPS_VALID_COURSE_BIT); - - gps_imcu.pacc = DL_IMCU_REMOTE_GPS_pacc(imcu_msg_buf); - gps_imcu.sacc = DL_IMCU_REMOTE_GPS_sacc(imcu_msg_buf); - gps_imcu.num_sv = DL_IMCU_REMOTE_GPS_numsv(imcu_msg_buf); - gps_imcu.fix = DL_IMCU_REMOTE_GPS_fix(imcu_msg_buf); - - // set gps msg time - gps_imcu.last_msg_ticks = sys_time.nb_sec_rem; - gps_imcu.last_msg_time = sys_time.nb_sec; - - if (gps_imcu.fix >= GPS_FIX_3D) { - gps_imcu.last_3dfix_ticks = sys_time.nb_sec_rem; - gps_imcu.last_3dfix_time = sys_time.nb_sec; - } - - AbiSendMsgGPS(GPS_IMCU_ID, now_ts, &gps_imcu); - break; - } - - #endif - - default: - break; - } -#if PPRZLINK_DEFAULT_VER == 2 - } -#endif + fbw_status.rc_status = DL_IMCU_FBW_STATUS_rc_status(buf); + fbw_status.frame_rate = DL_IMCU_FBW_STATUS_frame_rate(buf); + fbw_status.mode = DL_IMCU_FBW_STATUS_mode(buf); + electrical.vsupply = DL_IMCU_FBW_STATUS_vsupply(buf); + electrical.current = DL_IMCU_FBW_STATUS_current(buf); } -#pragma GCC diagnostic pop -/* Radio control event misused as InterMCU event for frame_handler */ -void RadioControlEvent(void (*frame_handler)(void)) -{ - /* Parse incoming bytes */ - if (intermcu.enabled) { - pprz_check_and_parse(intermcu.device, &intermcu.transport, imcu_msg_buf, &intermcu.msg_available); - - if (intermcu.msg_available) { - intermcu_parse_msg(frame_handler); - intermcu.msg_available = false; - } - } -} diff --git a/sw/airborne/modules/intermcu/intermcu_ap.h b/sw/airborne/modules/intermcu/intermcu_ap.h index dc5d12887b..40e6eccf61 100644 --- a/sw/airborne/modules/intermcu/intermcu_ap.h +++ b/sw/airborne/modules/intermcu/intermcu_ap.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2015 Freek van Tienen + * Copyright (C) 2022 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,46 +15,43 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ /** @file modules/intermcu/intermcu_ap.h - * @brief Rotorcraft Inter-MCU on the autopilot + * @brief Inter-MCU on the AP side */ -#ifndef INTERMCU_AP_ROTORCRAFT_H -#define INTERMCU_AP_ROTORCRAFT_H +#ifndef INTERMCU_AP_H +#define INTERMCU_AP_H #include "modules/intermcu/intermcu.h" #include "generated/airframe.h" -#include "modules/energy/electrical.h" -void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode); -void RadioControlEvent(void (*frame_handler)(void)); -void intermcu_send_spektrum_bind(void); -void intermcu_set_enabled(bool value); +/** send command vector over intermcu link instead of actuators + */ +extern void intermcu_send_commands(pprz_t *command_values, uint8_t ap_mode); -/* We need radio defines for the Autopilot */ -#define RADIO_THROTTLE 0 -#define RADIO_ROLL 1 -#define RADIO_PITCH 2 -#define RADIO_YAW 3 -#define RADIO_MODE 4 -#define RADIO_KILL_SWITCH 5 -#define RADIO_AUX1 5 -#define RADIO_AUX2 6 -#define RADIO_AUX3 7 -#define RADIO_CONTROL_NB_CHANNEL 8 +/** send binding signal for spektrum receiver + */ +extern void intermcu_send_spektrum_bind(void); + +/** enable/disable intermcu link + */ +extern void intermcu_set_enabled(bool value); + +/** Datalink event functions + */ +extern void intermcu_parse_IMCU_FBW_STATUS(uint8_t *buf); /* Structure for FBW status */ struct fbw_status_t { uint8_t rc_status; uint8_t frame_rate; uint8_t mode; - struct Electrical electrical; }; -#endif /* INTERMCU_AP_ROTORCRAFT_H */ +#endif /* INTERMCU_AP_H */ + diff --git a/sw/airborne/modules/intermcu/intermcu_fbw.c b/sw/airborne/modules/intermcu/intermcu_fbw.c index 819e0015a5..46db2e1f3c 100644 --- a/sw/airborne/modules/intermcu/intermcu_fbw.c +++ b/sw/airborne/modules/intermcu/intermcu_fbw.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2015 Freek van Tienen + * Copyright (C) 2022 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,24 +15,26 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ /** @file modules/intermcu/intermcu_fbw.c - * @brief Rotorcraft Inter-MCU on FlyByWire + * @brief Inter-MCU on FlyByWire side */ -#define ABI_C - -#include "intermcu_fbw.h" +#include "modules/intermcu/intermcu_fbw.h" +#include "main_fbw.h" #include "pprzlink/intermcu_msg.h" +#include "generated/modules.h" #include "modules/radio_control/radio_control.h" #include "modules/energy/electrical.h" -#include "mcu_periph/uart.h" -#include "modules/telemetry/telemetry_intermcu.h" +#if TELEMETRY_INTERMCU +#include "modules/datalink/intermcu_dl.h" +#endif +#include "modules/datalink/datalink.h" +#include "modules/core/abi.h" #include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" @@ -47,8 +50,8 @@ tid_t px4bl_tid; ///< id for time out of the px4 bootloader reset #endif #if RADIO_CONTROL_NB_CHANNEL > 8 -#undef RADIO_CONTROL_NB_CHANNEL -#define RADIO_CONTROL_NB_CHANNEL 8 +//#undef RADIO_CONTROL_NB_CHANNEL +//#define RADIO_CONTROL_NB_CHANNEL 8 INFO("InterMCU UART will only send 8 radio channels!") #endif @@ -58,24 +61,29 @@ struct intermcu_t intermcu = { .enabled = true, .msg_available = false }; -uint8_t imcu_msg_buf[128] __attribute__((aligned)); ///< The InterMCU message buffer +uint8_t imcu_msg_buf[256] __attribute__((aligned)); ///< The InterMCU message buffer pprz_t intermcu_commands[COMMANDS_NB]; -bool autopilot_motors_on = false; -static void intermcu_parse_msg(void (*commands_frame_handler)(void)); +bool intermcu_ap_motors_on = false; + #ifdef BOARD_PX4IO static void checkPx4RebootCommand(unsigned char b); #endif -#ifdef USE_GPS +// ABI callback for radio control +#ifndef IMCU_RADIO_CONTROL_ID +#define IMCU_RADIO_CONTROL_ID ABI_BROADCAST +#endif +static abi_event rc_ev; +static void rc_cb(uint8_t sender_id, struct RadioControl *rc); +// ABI callback for GPS +#ifdef USE_GPS +#include "modules/gps/gps.h" #ifndef IMCU_GPS_ID #define IMCU_GPS_ID GPS_MULTI_ID #endif - -#include "modules/core/abi.h" -#include "modules/gps/gps.h" static abi_event gps_ev; static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s); #endif @@ -84,6 +92,7 @@ void intermcu_init(void) { pprz_transport_init(&intermcu.transport); + AbiBindMsgRADIO_CONTROL(IMCU_RADIO_CONTROL_ID, &rc_ev, rc_cb); #if USE_GPS AbiBindMsgGPS(IMCU_GPS_ID, &gps_ev, gps_cb); #endif @@ -103,8 +112,100 @@ void intermcu_periodic(void) } } -void intermcu_on_rc_frame(uint8_t fbw_mode) +void intermcu_event(void) { + uint8_t i, c; + + // Check if there are messages in the device + if (intermcu.device->char_available(intermcu.device->periph)) { + while (intermcu.device->char_available(intermcu.device->periph) && !intermcu.transport.trans_rx.msg_received) { + c = intermcu.device->get_byte(intermcu.device->periph); + parse_pprz(&intermcu.transport, c); + +#ifdef BOARD_PX4IO + // TODO: create a hook + checkPx4RebootCommand(c); +#endif + } + + // If we have a message copy it + if (intermcu.transport.trans_rx.msg_received) { + for (i = 0; i < intermcu.transport.trans_rx.payload_len; i++) { + imcu_msg_buf[i] = intermcu.transport.trans_rx.payload[i]; + } + + intermcu.msg_available = true; + intermcu.transport.trans_rx.msg_received = false; + } + } + + if (intermcu.msg_available) { + uint8_t class_id = pprzlink_get_msg_class_id(imcu_msg_buf); + if (class_id == DL_intermcu_CLASS_ID) { + // parse intermcu messages and call callbacks + dl_parse_msg(intermcu.device, &intermcu.transport.trans_tx, imcu_msg_buf); +#if TELEMETRY_INTERMCU + } else { + // forward all other messages if needed + intermcu_dl_on_msg(imcu_msg_buf, intermcu.transport.trans_rx.payload_len); +#endif + } + intermcu.msg_available = false; + } +} + +void intermcu_send_status(void) +{ +#ifdef RADIO_CONTROL + uint8_t rc_status = radio_control.status; + uint8_t rc_rate = radio_control.frame_rate; +#else + uint8_t rc_status = 0; + uint8_t rc_rate = 0; +#endif + // Send Status + pprz_msg_send_IMCU_FBW_STATUS(&(intermcu.transport.trans_tx), intermcu.device, AC_ID, + &fbw_mode, &rc_status, &rc_rate, + &electrical.vsupply, &electrical.current); +} + +void intermcu_parse_IMCU_COMMANDS(uint8_t *buf) +{ + uint8_t size = DL_IMCU_COMMANDS_values_length(buf); + int16_t *new_commands = DL_IMCU_COMMANDS_values(buf); + intermcu.cmd_status |= DL_IMCU_COMMANDS_status(buf); + + // Read the autopilot status and then clear it + intermcu_ap_motors_on = INTERMCU_GET_CMD_STATUS(INTERMCU_CMD_MOTORS_ON); + INTERMCU_CLR_CMD_STATUS(INTERMCU_CMD_MOTORS_ON); + + for (int i = 0; i < size; i++) { + intermcu_commands[i] = new_commands[i]; + } + + intermcu.status = INTERMCU_OK; + intermcu.time_since_last_frame = 0; +} + +void intermcu_parse_IMCU_SPEKTRUM_SOFT_BIND(uint8_t *buf __attribute__((unused))) +{ +#if defined(SPEKTRUM_HAS_SOFT_BIND_PIN) + received_spektrum_soft_bind(); +#endif +} + +void intermcu_forward_uplink(uint8_t *buf __attribute__((unused))) +{ + // forward all incoming messages to intermcu for AP side +#if TELEMETRY_INTERMCU + uint8_t size = (DOWNLINK_TRANSPORT).trans_rx.payload_len; // FIXME is it always a valid length ? + intermcu_dl_repack(&(intermcu.transport.trans_tx), intermcu.device, buf, size); +#endif +} + +static void rc_cb(uint8_t sender_id __attribute__((unused)), struct RadioControl *rc __attribute__((unused))) +{ +#if RADIO_CONTROL pprz_t values[9]; values[INTERMCU_RADIO_THROTTLE] = radio_control.values[RADIO_THROTTLE]; @@ -133,100 +234,8 @@ void intermcu_on_rc_frame(uint8_t fbw_mode) #endif pprz_msg_send_IMCU_RADIO_COMMANDS(&(intermcu.transport.trans_tx), intermcu.device, - INTERMCU_FBW, &fbw_mode, RADIO_CONTROL_NB_CHANNEL, values); -} - -void intermcu_send_status(uint8_t mode) -{ - // Send Status - pprz_msg_send_IMCU_FBW_STATUS(&(intermcu.transport.trans_tx), intermcu.device, INTERMCU_FBW, - &mode, &(radio_control.status), &(radio_control.frame_rate), &electrical.vsupply, - &electrical.current); -} - -#pragma GCC diagnostic ignored "-Wcast-align" -static void intermcu_parse_msg(void (*commands_frame_handler)(void)) -{ - /* Parse the Inter MCU message */ - uint8_t msg_id = pprzlink_get_msg_id(imcu_msg_buf); -#if PPRZLINK_DEFAULT_VER == 2 - // Check that the message is really a intermcu message - if (pprzlink_get_msg_class_id(imcu_msg_buf) == DL_intermcu_CLASS_ID) { + INTERMCU_FBW, &fbw_mode, INTERMCU_RADIO_CONTROL_NB_CHANNEL, values); #endif - switch (msg_id) { - case DL_IMCU_COMMANDS: { - uint8_t i; - uint8_t size = DL_IMCU_COMMANDS_values_length(imcu_msg_buf); - int16_t *new_commands = DL_IMCU_COMMANDS_values(imcu_msg_buf); - intermcu.cmd_status |= DL_IMCU_COMMANDS_status(imcu_msg_buf); - - // Read the autopilot status and then clear it - autopilot_motors_on = INTERMCU_GET_CMD_STATUS(INTERMCU_CMD_MOTORS_ON); - INTERMCU_CLR_CMD_STATUS(INTERMCU_CMD_MOTORS_ON) - - for (i = 0; i < size; i++) { - intermcu_commands[i] = new_commands[i]; - } - - intermcu.status = INTERMCU_OK; - intermcu.time_since_last_frame = 0; - commands_frame_handler(); - break; - } - #if defined(TELEMETRY_INTERMCU_DEV) - case DL_IMCU_TELEMETRY: { - uint8_t size = DL_IMCU_TELEMETRY_msg_length(imcu_msg_buf); - uint8_t *msg = DL_IMCU_TELEMETRY_msg(imcu_msg_buf); - telemetry_intermcu_on_msg(msg, size); - break; - } - #endif - #if defined(SPEKTRUM_HAS_SOFT_BIND_PIN) //TODO: make subscribable module parser - case DL_IMCU_SPEKTRUM_SOFT_BIND: - received_spektrum_soft_bind(); - break; - #endif - - default: - break; - } -#if PPRZLINK_DEFAULT_VER == 2 - } -#endif -} -#pragma GCC diagnostic pop - -void InterMcuEvent(void (*frame_handler)(void)) -{ - uint8_t i, c; - - // Check if there are messages in the device - if (intermcu.device->char_available(intermcu.device->periph)) { - while (intermcu.device->char_available(intermcu.device->periph) && !intermcu.transport.trans_rx.msg_received) { - c = intermcu.device->get_byte(intermcu.device->periph); - parse_pprz(&intermcu.transport, c); - -#ifdef BOARD_PX4IO - // TODO: create a hook - checkPx4RebootCommand(c); -#endif - } - - // If we have a message copy it - if (intermcu.transport.trans_rx.msg_received) { - for (i = 0; i < intermcu.transport.trans_rx.payload_len; i++) { - imcu_msg_buf[i] = intermcu.transport.trans_rx.payload[i]; - } - - intermcu.msg_available = true; - intermcu.transport.trans_rx.msg_received = false; - } - } - - if (intermcu.msg_available) { - intermcu_parse_msg(frame_handler); - intermcu.msg_available = false; - } } #if USE_GPS diff --git a/sw/airborne/modules/intermcu/intermcu_fbw.h b/sw/airborne/modules/intermcu/intermcu_fbw.h index cf6fb8599b..0f61adacbf 100644 --- a/sw/airborne/modules/intermcu/intermcu_fbw.h +++ b/sw/airborne/modules/intermcu/intermcu_fbw.h @@ -14,38 +14,49 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ /** @file modules/intermcu/intermcu_fbw.h - * @brief Rotorcraft Inter-MCU on FlyByWire + * @brief Inter-MCU on FlyByWire side */ -#ifndef INTERMCU_FBW_ROTORCRAFT_H -#define INTERMCU_FBW_ROTORCRAFT_H +#ifndef INTERMCU_FBW_H +#define INTERMCU_FBW_H #include "modules/intermcu/intermcu.h" +#include "pprzlink/intermcu_msg.h" -extern bool autopilot_motors_on; +extern bool intermcu_ap_motors_on; extern pprz_t intermcu_commands[COMMANDS_NB]; -void intermcu_on_rc_frame(uint8_t fbw_mode); -void intermcu_send_status(uint8_t mode); -void InterMcuEvent(void (*frame_handler)(void)); + +/** send fbw status message + */ +extern void intermcu_send_status(void); + +/** Datalink event functions + */ +extern void intermcu_parse_IMCU_COMMANDS(uint8_t *buf); +extern void intermcu_parse_IMCU_SPEKTRUM_SOFT_BIND(uint8_t *buf); +extern void intermcu_forward_uplink(uint8_t *buf); -/* We need radio defines for the Autopilot */ -#define INTERMCU_RADIO_THROTTLE 0 -#define INTERMCU_RADIO_ROLL 1 -#define INTERMCU_RADIO_PITCH 2 -#define INTERMCU_RADIO_YAW 3 -#define INTERMCU_RADIO_MODE 4 -#define INTERMCU_RADIO_KILL_SWITCH 5 -#define INTERMCU_RADIO_AUX1 5 -#define INTERMCU_RADIO_AUX2 6 -#define INTERMCU_RADIO_AUX3 7 +/* We need radio defines for the Autopilot + * index are matching rc_intermcu.h + */ +#define INTERMCU_RADIO_THROTTLE 0 +#define INTERMCU_RADIO_ROLL 1 +#define INTERMCU_RADIO_PITCH 2 +#define INTERMCU_RADIO_YAW 3 +#define INTERMCU_RADIO_MODE 4 +#define INTERMCU_RADIO_KILL_SWITCH 5 +#define INTERMCU_RADIO_AUX1 5 // be careful with ID 5 +#define INTERMCU_RADIO_AUX2 6 +#define INTERMCU_RADIO_AUX3 7 + #define INTERMCU_RADIO_CONTROL_NB_CHANNEL 8 -#endif /* INTERMCU_FBW_ROTORCRAFT_H */ +#endif /* INTERMCU_FB_H */ + diff --git a/sw/airborne/modules/intermcu/link_mcu_can.c b/sw/airborne/modules/intermcu/link_mcu_can.c deleted file mode 100644 index a65734155b..0000000000 --- a/sw/airborne/modules/intermcu/link_mcu_can.c +++ /dev/null @@ -1,232 +0,0 @@ -/* - * Copyright (C) 2010-2012 The Paparazzi Team - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "modules/intermcu/link_mcu_can.h" -#include "mcu_periph/can.h" -#include "led.h" - -#include "modules/core/commands.h" - - -////////////////////////////////////////////////////////////////////////////////////////////// -// INTERMCU CAN MESSAGES - -// Commands -#define MSG_INTERMCU_COMMAND_MASTER_ID 0x01 -#define MSG_INTERMCU_COMMAND_EXTRA_ID 0x02 -// Channels -#define MSG_INTERMCU_RADIO_LOW_ID 0x04 -#define MSG_INTERMCU_RADIO_HIGH_ID 0x05 -// Trim -#define MSG_INTERMCU_TRIM_ID 0x09 -// Status -#define MSG_INTERMCU_FBW_STATUS_ID 0x10 - - -union { - uint8_t data[8]; - pprz_t cmd[4]; -} imcu_cmd_mstr, imcu_cmd_ext, imcu_chan1, imcu_chan2, imcu_trim; - -#define INTERMCU_COMMAND(_intermcu_payload, nr) (pprz_t)((uint16_t)(*((uint8_t*)_intermcu_payload+0+(2*(nr)))|*((uint8_t*)_intermcu_payload+1+(2*(nr)))<<8)) - -////////////////////////////////////////////////////////////////////////////////////////////// -// READ MESSAGES - - -void link_mcu_on_can_msg(uint32_t id, uint8_t *data, int len); -void link_mcu_on_can_msg(uint32_t id, uint8_t *data, int len) -{ -#if COMMANDS_NB > 8 -#error "INTERMCU_CAN CAN ONLY SEND 4 OR 8 COMMANDS (packets of 8 bytes)" -#endif -#if RADIO_CONTROL_NB_CHANNEL > 8 -#warning "INTERMCU_CAN CAN ONLY SEND 8 RADIO CHANNELS: CHANNELS 9 and higher will not be sent" -#endif - if (len) {} //Remove compile warning - - if (id == MSG_INTERMCU_COMMAND_MASTER_ID) { - for (int i = 0; (i < 4) && (i < COMMANDS_NB); i++) { - ap_state->commands[i] = INTERMCU_COMMAND(data, i); - } -#ifdef LINK_MCU_LED - LED_TOGGLE(LINK_MCU_LED); -#endif - inter_mcu_received_ap = true; - } - - if (id == MSG_INTERMCU_COMMAND_EXTRA_ID) { - for (int i = 0; (i < 4) && (i < (COMMANDS_NB - 4)); i++) { - ap_state->commands[4 + i] = INTERMCU_COMMAND(data, i); - } - } - - - if (id == MSG_INTERMCU_TRIM_ID) { - ap_state->command_roll_trim = ((pprz_t) INTERMCU_COMMAND(data, 0)); - ap_state->command_pitch_trim = ((pprz_t) INTERMCU_COMMAND(data, 1)); - } - - if (id == MSG_INTERMCU_RADIO_LOW_ID) { - for (int i = 0; (i < 4) && (i < RADIO_CONTROL_NB_CHANNEL); i++) { - fbw_state->channels[i] = ((pprz_t)INTERMCU_COMMAND(data, i)); - } - } - - if (id == MSG_INTERMCU_RADIO_HIGH_ID) { - for (int i = 0; (i < 4) && (i < (RADIO_CONTROL_NB_CHANNEL - 4)); i++) { - fbw_state->channels[4 + i] = ((pprz_t)INTERMCU_COMMAND(data, i)); - } - } - - if ((id == MSG_INTERMCU_FBW_STATUS_ID) && (len == 5)) { - fbw_state->ppm_cpt = data[0]; - fbw_state->status = data[1]; - fbw_state->nb_err = data[2]; - fbw_state->electrical.vsupply = (float)(data[3] + (data[4] << 8))/10.f; - fbw_state->electrical.current = 0; - -#ifdef LINK_MCU_LED - LED_TOGGLE(LINK_MCU_LED); -#endif - inter_mcu_received_fbw = true; - } -} - -////////////////////////////////////////////////////////////////////////////////////////////// -// SEND MESSAGES - - -#ifdef AP -void link_mcu_send(void) -{ - for (int i = 0; (i < COMMANDS_NB) && (i < 4); i++) { - imcu_cmd_mstr.cmd[i] = ap_state->commands[i]; - } - for (int i = 0; (i < (COMMANDS_NB - 4)) && (i < 4); i++) { - imcu_cmd_ext.cmd[i] = ap_state->commands[4 + i]; - } - - ppz_can_transmit(MSG_INTERMCU_COMMAND_MASTER_ID, imcu_cmd_mstr.data, 8); - ppz_can_transmit(MSG_INTERMCU_COMMAND_EXTRA_ID, imcu_cmd_ext.data, 8); - - imcu_trim.cmd[0] = ap_state->command_roll_trim; - imcu_trim.cmd[1] = ap_state->command_pitch_trim; - imcu_trim.cmd[2] = ap_state->command_yaw_trim; - RunOnceEvery(6, ppz_can_transmit(MSG_INTERMCU_TRIM_ID, imcu_trim.data, 6)); -} -#endif - -#ifdef FBW -void link_mcu_periodic_task(void) -{ - // Import: Prepare the next message for AP - inter_mcu_fill_fbw_state(); - - // Transmit Status - uint8_t intermcu_tx_buff[8]; - intermcu_tx_buff[0] = fbw_state->ppm_cpt; - intermcu_tx_buff[1] = fbw_state->status; - intermcu_tx_buff[2] = fbw_state->nb_err; - uint16_t vsupply = fbw_state->electrical.vsupply * 10; - intermcu_tx_buff[3] = (uint8_t) vsupply; - intermcu_tx_buff[4] = (uint8_t)((vsupply & 0xff00) >> 8); - ppz_can_transmit(MSG_INTERMCU_FBW_STATUS_ID, intermcu_tx_buff, 5); - -#if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1 - // Copy the CHANNELS to the 2 CAN buffers - for (int i = 0; (i < RADIO_CONTROL_NB_CHANNEL) && (i < 4); i++) { - imcu_chan1.cmd[i] = fbw_state->channels[i]; - } - for (int i = 0; (i < (RADIO_CONTROL_NB_CHANNEL - 4)) && (i < 4); i++) { - imcu_chan2.cmd[i] = fbw_state->channels[4 + i]; - } - - if (bit_is_set(fbw_state->status, RC_OK)) { - ppz_can_transmit(MSG_INTERMCU_RADIO_LOW_ID, imcu_chan1.data, 8); - ppz_can_transmit(MSG_INTERMCU_RADIO_HIGH_ID, imcu_chan2.data, 8); - } -#endif -} -#endif - -////////////////////////////////////////////////////////////////////////////////////////////// -// STATUS LOGIC AND TELEMETRY -// Downlink FBW status from AP - -struct link_mcu_msg link_mcu_from_ap_msg; -struct link_mcu_msg link_mcu_from_fbw_msg; - - -#ifdef AP -#include "modules/datalink/telemetry.h" - -#define RC_OK 0 -#define RC_LOST 1 -#define RC_REALLY_LOST 2 - - -static void send_commands(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, ap_state->commands); -} - -static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) -{ - uint8_t rc_status = 0; - uint8_t fbw_status = 0; - if (bit_is_set(fbw_state->status, STATUS_MODE_AUTO)) { - fbw_status = FBW_MODE_AUTO; - } - if (bit_is_set(fbw_state->status, STATUS_MODE_FAILSAFE)) { - fbw_status = FBW_MODE_FAILSAFE; - } - if (bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)) { - rc_status = RC_REALLY_LOST; - } else if (bit_is_set(fbw_state->status, RC_OK)) { - rc_status = RC_OK; - } else { - rc_status = RC_LOST; - } - pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, - &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), - &(fbw_state->electrical.vsupply), &(fbw_state->electrical.current)); -} -#endif - -void link_mcu_init(void) -{ - ppz_can_init((can_rx_callback_t)link_mcu_on_can_msg); - -#ifdef AP -#if PERIODIC_TELEMETRY - // If FBW has not telemetry, then AP can send some of the info - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands); - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_fbw_status); -#endif -#endif -} - -void link_mcu_event_task(void) -{ - // No event function: CAN_RX_IRQ is called on reception of data -} diff --git a/sw/airborne/modules/intermcu/link_mcu_can.h b/sw/airborne/modules/intermcu/link_mcu_can.h deleted file mode 100644 index 9234ec9ee6..0000000000 --- a/sw/airborne/modules/intermcu/link_mcu_can.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Copyright (C) 2010-2012 The Paparazzi Team - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - */ - -/** - * @file modules/intermcu/link_mcu_can.h - * Transport for the communication between FBW and AP via CAN. - * - */ - -#ifndef LINK_MCU_CAN_H -#define LINK_MCU_CAN_H - -#include -#include "modules/intermcu/inter_mcu.h" - -struct link_mcu_msg { - union { - struct fbw_state from_fbw; - struct ap_state from_ap; - } payload; -}; - -extern struct link_mcu_msg link_mcu_from_ap_msg; -extern struct link_mcu_msg link_mcu_from_fbw_msg; - -extern bool link_mcu_received; - -extern void link_mcu_send(void); -extern void link_mcu_init(void); -extern void link_mcu_event_task(void); -extern void link_mcu_periodic_task(void); - - -#endif /* LINK_MCU_CAN_H */ diff --git a/sw/airborne/modules/intermcu/link_mcu_spi.c b/sw/airborne/modules/intermcu/link_mcu_spi.c deleted file mode 100644 index e77a8a7dfc..0000000000 --- a/sw/airborne/modules/intermcu/link_mcu_spi.c +++ /dev/null @@ -1,168 +0,0 @@ -/* - * Copyright (C) 2003-2006 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "modules/intermcu/link_mcu_spi.h" - -#ifndef LINK_MCU_SPI_DEV -#define LINK_MCU_SPI_DEV spi1 -#endif - -struct link_mcu_msg link_mcu_from_ap_msg; -struct link_mcu_msg link_mcu_from_fbw_msg; - -struct spi_transaction link_mcu_trans; - -bool link_mcu_received; - -static uint16_t crc = 0; - -#define PAYLOAD_LENGTH sizeof(link_mcu_from_fbw_msg.payload) -#define LINK_MCU_FRAME_LENGTH sizeof(struct link_mcu_msg) - -#define ComputeChecksum(_buf) { \ - uint8_t i; \ - crc = CRC_INIT; \ - for(i = 0; i < PAYLOAD_LENGTH; i++) { \ - uint8_t _byte = ((uint8_t*)&_buf)[i]; \ - crc = CrcUpdate(crc, _byte); \ - } \ - } - -#ifdef FBW - -void link_mcu_init(void) -{ - - link_mcu_trans.cpol = SPICpolIdleLow; - link_mcu_trans.cpha = SPICphaEdge2; - link_mcu_trans.dss = SPIDss8bit; - link_mcu_trans.input_buf = (uint8_t *)&link_mcu_from_ap_msg; - link_mcu_trans.output_buf = (uint8_t *)&link_mcu_from_fbw_msg; - link_mcu_trans.input_length = LINK_MCU_FRAME_LENGTH; - link_mcu_trans.output_length = LINK_MCU_FRAME_LENGTH; - spi_slave_register(&(LINK_MCU_SPI_DEV), &link_mcu_trans); -} - -void link_mcu_restart(void) -{ - ComputeChecksum(link_mcu_from_fbw_msg); - link_mcu_from_fbw_msg.checksum = crc; - - // wait for the next transaction - spi_slave_wait(&(LINK_MCU_SPI_DEV)); -} - -void link_mcu_event_task(void) -{ - if (link_mcu_trans.status == SPITransSuccess) { - /* Got a message on SPI. */ - link_mcu_trans.status = SPITransDone; - - /* A message has been received */ - ComputeChecksum(link_mcu_from_ap_msg); - link_mcu_received = true; - if (link_mcu_from_ap_msg.checksum == crc) { - inter_mcu_received_ap = true; - } else { - fbw_state->nb_err++; - } - } - if (link_mcu_trans.status == SPITransFailed) { - link_mcu_trans.status = SPITransDone; - link_mcu_received = true; - fbw_state->nb_err++; - } -} - -#endif /* FBW */ - - - -/*****************************************************************************/ -#ifdef AP - -#ifndef LINK_MCU_SLAVE_IDX -#define LINK_MCU_SLAVE_IDX SPI_SLAVE0 -#endif - -uint8_t link_mcu_nb_err; -uint8_t link_mcu_fbw_nb_err; - -#if PERIODIC_TELEMETRY -#include "modules/datalink/telemetry.h" - -static void send_debug_link(struct transport_tx *trans, struct link_device *dev) -{ - uint8_t mcu1_ppm_cpt_foo = 0; //FIXME - pprz_msg_send_DEBUG_MCU_LINK(trans, dev, AC_ID, - &link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt_foo); -} -#endif - -void link_mcu_init(void) -{ - link_mcu_nb_err = 0; - - link_mcu_trans.cpol = SPICpolIdleLow; - link_mcu_trans.cpha = SPICphaEdge2; - link_mcu_trans.dss = SPIDss8bit; - link_mcu_trans.select = SPISelectUnselect; - link_mcu_trans.slave_idx = LINK_MCU_SLAVE_IDX; - link_mcu_trans.input_buf = (uint8_t *)&link_mcu_from_fbw_msg; - link_mcu_trans.output_buf = (uint8_t *)&link_mcu_from_ap_msg; - link_mcu_trans.input_length = LINK_MCU_FRAME_LENGTH; - link_mcu_trans.output_length = LINK_MCU_FRAME_LENGTH; - -#if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DEBUG_MCU_LINK, send_debug_link); -#endif -} - -void link_mcu_send(void) -{ - - ComputeChecksum(link_mcu_from_ap_msg); - link_mcu_from_ap_msg.checksum = crc; - spi_submit(&(LINK_MCU_SPI_DEV), &link_mcu_trans); -} - -void link_mcu_event_task(void) -{ - if (link_mcu_trans.status == SPITransSuccess) { - /* Got a message on SPI. */ - link_mcu_trans.status = SPITransDone; - /* A message has been received */ - ComputeChecksum(link_mcu_from_fbw_msg); - if (link_mcu_from_fbw_msg.checksum == crc) { - inter_mcu_received_fbw = true; - } else { - link_mcu_nb_err++; - } - } - if (link_mcu_trans.status == SPITransFailed) { - link_mcu_trans.status = SPITransDone; - link_mcu_received = true; - link_mcu_nb_err++; - } -} - -#endif /* AP */ diff --git a/sw/airborne/modules/intermcu/link_mcu_spi.h b/sw/airborne/modules/intermcu/link_mcu_spi.h deleted file mode 100644 index dae3dcd48c..0000000000 --- a/sw/airborne/modules/intermcu/link_mcu_spi.h +++ /dev/null @@ -1,67 +0,0 @@ -/* - * Copyright (C) 2003-2005 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - */ - -/** - * @file modules/intermcu/link_mcu_spi.h - * Transport for the communication between FBW and AP via SPI. - * - */ - -#ifndef LINK_MCU_SPI_H -#define LINK_MCU_SPI_H - -#include -#include "modules/intermcu/inter_mcu.h" -#include "mcu_periph/spi.h" - -#ifndef SITL -#include "modules/intermcu/link_mcu_hw.h" -#endif - -struct link_mcu_msg { - union { - struct fbw_state from_fbw; - struct ap_state from_ap; - } payload; - uint16_t checksum; -}; - -extern struct link_mcu_msg link_mcu_from_ap_msg; -extern struct link_mcu_msg link_mcu_from_fbw_msg; - -extern struct spi_transaction link_mcu_trans; - -extern bool link_mcu_received; - -extern void link_mcu_init(void); -extern void link_mcu_event_task(void); - -#ifdef FBW -extern void link_mcu_restart(void); -#endif /* FBW */ - -#ifdef AP -extern uint8_t link_mcu_nb_err; -extern uint8_t link_mcu_fbw_nb_err; - -extern void link_mcu_send(void); -#endif /* AP */ - -#endif /* LINK_MCU_SPI_H */ diff --git a/sw/airborne/modules/intermcu/link_mcu_usart.c b/sw/airborne/modules/intermcu/link_mcu_usart.c deleted file mode 100644 index ed3fdd76ea..0000000000 --- a/sw/airborne/modules/intermcu/link_mcu_usart.c +++ /dev/null @@ -1,386 +0,0 @@ -/* - * Copyright (C) 2010-2012 The Paparazzi Team - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "modules/intermcu/link_mcu_usart.h" -#include "mcu_periph/uart.h" -#include "led.h" - -#include "modules/core/commands.h" - -////////////////////////////////////////////////////////////////////////////////////////////// -// LINK - -// Use uart interface directly -#define InterMcuBuffer() uart_char_available(&(INTERMCU_LINK)) -#define InterMcuUartSend1(c) uart_put_byte(&(INTERMCU_LINK), 0, c) -#define InterMcuUartSetBaudrate(_a) uart_periph_set_baudrate(&(INTERMCU_LINK), _a) -#define InterMcuUartSendMessage() {} -#define InterMcuUartGetch() uart_getch(&(INTERMCU_LINK)) - -////////////////////////////////////////////////////////////////////////////////////////////// -// PROTOCOL - -#define INTERMCU_SYNC1 0xB5 -#define INTERMCU_SYNC2 0x62 - -#define InterMcuInitCheksum() { intermcu_data.send_ck_a = intermcu_data.send_ck_b = 0; } -#define InterMcuUpdateChecksum(c) { intermcu_data.send_ck_a += c; intermcu_data.send_ck_b += intermcu_data.send_ck_a; } -#define InterMcuTrailer() { InterMcuUartSend1(intermcu_data.send_ck_a); InterMcuUartSend1(intermcu_data.send_ck_b); InterMcuUartSendMessage(); } - -#define InterMcuSend1(c) { uint8_t i8=c; InterMcuUartSend1(i8); InterMcuUpdateChecksum(i8); } -#define InterMcuSend2(c) { uint16_t i16=c; InterMcuSend1(i16&0xff); InterMcuSend1(i16 >> 8); } -#define InterMcuSend1ByAddr(x) { InterMcuSend1(*x); } -#define InterMcuSend2ByAddr(x) { InterMcuSend1(*x); InterMcuSend1(*(x+1)); } -#define InterMcuSend4ByAddr(x) { InterMcuSend1(*x); InterMcuSend1(*(x+1)); InterMcuSend1(*(x+2)); InterMcuSend1(*(x+3)); } - -#define InterMcuHeader(nav_id, msg_id, len) { \ - InterMcuUartSend1(INTERMCU_SYNC1); \ - InterMcuUartSend1(INTERMCU_SYNC2); \ - InterMcuInitCheksum(); \ - InterMcuSend1(nav_id); \ - InterMcuSend1(msg_id); \ - InterMcuSend2(len); \ - } - -////////////////////////////////////////////////////////////////////////////////////////////// -// MESSAGES - -// class -#define MSG_INTERMCU_ID 100 - - -#define MSG_INTERMCU_COMMAND_ID 0x05 -#define MSG_INTERMCU_COMMAND_LENGTH (2*(COMMANDS_NB)) -#define MSG_INTERMCU_COMMAND(_intermcu_payload, nr) (uint16_t)(*((uint8_t*)_intermcu_payload+0+(2*(nr)))|*((uint8_t*)_intermcu_payload+1+(2*(nr)))<<8) - -#define InterMcuSend_INTERMCU_COMMAND(cmd) { \ - InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_COMMAND_ID, MSG_INTERMCU_COMMAND_LENGTH);\ - for (int i=0;i INTERMCU_MAX_PAYLOAD) { - goto error; - } - intermcu_data.msg_idx = 0; - intermcu_data.status++; - break; - case LINK_MCU_GOT_LEN2: - intermcu_data.msg_buf[intermcu_data.msg_idx] = c; - intermcu_data.msg_idx++; - if (intermcu_data.msg_idx >= intermcu_data.len) { - intermcu_data.status++; - } - break; - case LINK_MCU_GOT_PAYLOAD: - if (c != intermcu_data.ck_a) { - goto error; - } - intermcu_data.status++; - break; - case LINK_MCU_GOT_CHECKSUM1: - if (c != intermcu_data.ck_b) { - goto error; - } - intermcu_data.msg_available = true; - goto restart; - break; - default: - goto error; - } - return; -error: - intermcu_data.error_cnt++; -restart: - intermcu_data.status = LINK_MCU_UNINIT; - return; -} - - - -////////////////////////////////////////////////////////////////////////////////////////////// -// USER - - -struct link_mcu_msg link_mcu_from_ap_msg; -struct link_mcu_msg link_mcu_from_fbw_msg; - -inline void parse_mavpilot_msg(void); - - -#ifdef AP -#include "modules/datalink/telemetry.h" - -#define RC_OK 0 -#define RC_LOST 1 -#define RC_REALLY_LOST 2 - - -static void send_commands(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, ap_state->commands); -} - - -static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) -{ - uint8_t rc_status = 0; - uint8_t fbw_status = 0; - if (bit_is_set(fbw_state->status, STATUS_MODE_AUTO)) { - fbw_status = FBW_MODE_AUTO; - } - if (bit_is_set(fbw_state->status, STATUS_MODE_FAILSAFE)) { - fbw_status = FBW_MODE_FAILSAFE; - } - if (bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)) { - rc_status = RC_REALLY_LOST; - } else if (bit_is_set(fbw_state->status, RC_OK)) { - rc_status = RC_OK; - } else { - rc_status = RC_LOST; - } - pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, - &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->electrical.vsupply), &(fbw_state->electrical.current)); -} -#endif - -void link_mcu_init(void) -{ - intermcu_data.status = LINK_MCU_UNINIT; - intermcu_data.msg_available = false; - intermcu_data.error_cnt = 0; -#ifdef AP -#if PERIODIC_TELEMETRY - // If FBW has not telemetry, then AP can send some of the info - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands); - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_fbw_status); -#endif -#endif -} - -void parse_mavpilot_msg(void) -{ - if (intermcu_data.msg_class == MSG_INTERMCU_ID) { - if (intermcu_data.msg_id == MSG_INTERMCU_COMMAND_ID) { -#if COMMANDS_NB > 8 -#error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED" -#endif - - for (int i = 0; i < COMMANDS_NB; i++) { - ap_state->commands[i] = ((pprz_t)MSG_INTERMCU_COMMAND(intermcu_data.msg_buf, i)); - } - -#ifdef LINK_MCU_LED - LED_TOGGLE(LINK_MCU_LED); -#endif - inter_mcu_received_ap = true; - } else if (intermcu_data.msg_id == MSG_INTERMCU_RADIO_ID) { -#if RADIO_CONTROL_NB_CHANNEL > 10 -#error "INTERMCU UART CAN ONLY SEND 10 RADIO CHANNELS OR THE UART WILL BE OVERFILLED" -#endif - - for (int i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) { - fbw_state->channels[i] = ((pprz_t)MSG_INTERMCU_RADIO(intermcu_data.msg_buf, i)); - } - } else if (intermcu_data.msg_id == MSG_INTERMCU_TRIM_ID) { - ap_state->command_roll_trim = ((pprz_t) MSG_INTERMCU_TRIM_ROLL(intermcu_data.msg_buf)); - ap_state->command_pitch_trim = ((pprz_t) MSG_INTERMCU_TRIM_PITCH(intermcu_data.msg_buf)); - } else if (intermcu_data.msg_id == MSG_INTERMCU_FBW_ID) { - fbw_state->ppm_cpt = MSG_INTERMCU_FBW_MOD(intermcu_data.msg_buf); - fbw_state->status = MSG_INTERMCU_FBW_STAT(intermcu_data.msg_buf); - fbw_state->nb_err = MSG_INTERMCU_FBW_ERR(intermcu_data.msg_buf); - fbw_state->electrical.vsupply = (float)(MSG_INTERMCU_FBW_VOLT(intermcu_data.msg_buf)) / 10.f; - fbw_state->electrical.current = (float)(MSG_INTERMCU_FBW_CURRENT(intermcu_data.msg_buf)) / 10.f; - -#ifdef LINK_MCU_LED - LED_TOGGLE(LINK_MCU_LED); -#endif - inter_mcu_received_fbw = true; - } - } -} - - -#ifdef AP -void link_mcu_send(void) -{ - InterMcuSend_INTERMCU_COMMAND(ap_state->commands); - InterMcuSend_INTERMCU_TRIM(ap_state->command_roll_trim, ap_state->command_pitch_trim); -} -#endif - -#ifdef FBW -// 60 Hz -static uint8_t SixtyHzCounter = 0; - -void link_mcu_periodic_task(void) -{ - SixtyHzCounter++; - if (SixtyHzCounter >= 3) { - // 20 Hz - SixtyHzCounter = 0; - inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */ - - InterMcuSend_INTERMCU_FBW( - fbw_state->ppm_cpt, - fbw_state->status, - fbw_state->nb_err, - fbw_state->electrical.vsupply * 10.f, - fbw_state->electrical.current * 10.f); -#if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1 - InterMcuSend_INTERMCU_RADIO(fbw_state->channels); -#endif - - } -} -#endif - -void link_mcu_event_task(void) -{ - /* A message has been received */ - if (InterMcuBuffer()) { - while (InterMcuBuffer()) { - intermcu_parse(InterMcuUartGetch()); - if (intermcu_data.msg_available) { - parse_mavpilot_msg(); - intermcu_data.msg_available = false; - } - } - } -} diff --git a/sw/airborne/modules/intermcu/link_mcu_usart.h b/sw/airborne/modules/intermcu/link_mcu_usart.h deleted file mode 100644 index d0741a3b93..0000000000 --- a/sw/airborne/modules/intermcu/link_mcu_usart.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Copyright (C) 2010-2012 The Paparazzi Team - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - */ - -/** - * @file modules/intermcu/link_mcu_usart.h - * Transport for the communication between FBW and AP via UART. - * - */ - -#ifndef LINK_MCU_USART_H -#define LINK_MCU_USART_H - -#include -#include "modules/intermcu/inter_mcu.h" - -struct link_mcu_msg { - union { - struct fbw_state from_fbw; - struct ap_state from_ap; - } payload; -}; - -extern struct link_mcu_msg link_mcu_from_ap_msg; -extern struct link_mcu_msg link_mcu_from_fbw_msg; - -extern bool link_mcu_received; - -extern void link_mcu_send(void); -extern void link_mcu_init(void); -extern void link_mcu_event_task(void); -extern void link_mcu_periodic_task(void); - - -#endif /* LINK_MCU_USART_H */ diff --git a/sw/airborne/modules/nav/nav_drop.c b/sw/airborne/modules/nav/nav_drop.c index 465d073353..a23bfdfa63 100644 --- a/sw/airborne/modules/nav/nav_drop.c +++ b/sw/airborne/modules/nav/nav_drop.c @@ -30,7 +30,7 @@ #include "state.h" #include "generated/flight_plan.h" #include "generated/airframe.h" -#include "modules/intermcu/inter_mcu.h" +#include "modules/core/commands.h" #if defined WP_RELEASE @@ -186,7 +186,7 @@ unit_t nav_drop_compute_approach(uint8_t wp_target, uint8_t wp_start, uint8_t wp unit_t nav_drop_shoot(void) { - imcu_set_command(COMMAND_HATCH, MAX_PPRZ); + command_set(COMMAND_HATCH, MAX_PPRZ); return 0; } diff --git a/sw/airborne/modules/nav/nav_drop.h b/sw/airborne/modules/nav/nav_drop.h index d698773012..0f03ef13af 100644 --- a/sw/airborne/modules/nav/nav_drop.h +++ b/sw/airborne/modules/nav/nav_drop.h @@ -42,7 +42,7 @@ extern bool compute_alignment(uint8_t w1, uint8_t w2, uint8_t start, uint8_t end #define NavDropComputeApproach(_target, _start, _radius) nav_drop_compute_approach(_target, _start, _radius) #define NavDropUpdateRelease(_wp) nav_drop_update_release(_wp) #define NavDropShoot() nav_drop_shoot() -#define NavDropCloseHatch() ({ imcu_set_command(COMMAND_HATCH, MIN_PPRZ); }) +#define NavDropCloseHatch() ({ command_set(COMMAND_HATCH, MIN_PPRZ); }) #define NavDropAligned() Qdr(DegOfRad(nav_drop_qdr_aligned)) #endif // NAV_DROP_H diff --git a/sw/airborne/modules/radio_control/cc2500_frsky/cc2500_paparazzi.c b/sw/airborne/modules/radio_control/cc2500_frsky/cc2500_paparazzi.c index e94046ef63..62dcf42961 100644 --- a/sw/airborne/modules/radio_control/cc2500_frsky/cc2500_paparazzi.c +++ b/sw/airborne/modules/radio_control/cc2500_frsky/cc2500_paparazzi.c @@ -22,6 +22,7 @@ #include "cc2500_paparazzi.h" #include "modules/radio_control/radio_control.h" +#include "modules/core/abi.h" #include "peripherals/cc2500.h" #include "cc2500_common.h" #include "cc2500_frsky_common.h" @@ -47,18 +48,19 @@ static void send_cc2500_ppm(struct transport_tx *trans, struct link_device *dev) #endif -void radio_control_impl_init(void) { +void radio_control_cc2500_init(void) { cc2500_settings_init(); cc2500_init(); cc2500Reset(); rxInit(); + radio_control.nb_channel = RADIO_CTL_NB; #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_PPM, send_cc2500_ppm); #endif } -void radio_control_impl_event(void (* _received_frame_handler)(void)) { +void radio_control_cc2500_event(void) { if (rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig) & RX_FRAME_COMPLETE) { rxRuntimeConfig.rcProcessFrameFn(&rxRuntimeConfig); radio_control.frame_cpt++; @@ -67,11 +69,12 @@ void radio_control_impl_event(void (* _received_frame_handler)(void)) { radio_control.radio_ok_cpt--; } else { radio_control.status = RC_OK; - for (int i = 0; i < RADIO_CONTROL_NB_CHANNEL; ++i) { + for (int i = 0; i < RADIO_CTL_NB; ++i) { frsky_raw[i] = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, i); } NormalizePpmIIR(frsky_raw, radio_control); - _received_frame_handler(); + AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_FRSKY_ID, &radio_control); } } } + diff --git a/sw/airborne/modules/radio_control/cc2500_frsky/cc2500_paparazzi.h b/sw/airborne/modules/radio_control/cc2500_frsky/cc2500_paparazzi.h index 5d8e400454..97ae343c02 100644 --- a/sw/airborne/modules/radio_control/cc2500_frsky/cc2500_paparazzi.h +++ b/sw/airborne/modules/radio_control/cc2500_frsky/cc2500_paparazzi.h @@ -33,8 +33,8 @@ #define RADIO_CONTROL_NB_CHANNEL RADIO_CTL_NB #endif -extern void radio_control_impl_event(void (* _received_frame_handler)(void)); -#define RadioControlEvent(_received_frame_handler) radio_control_impl_event(_received_frame_handler) - +extern void radio_control_cc2500_init(void); +extern void radio_control_cc2500_event(void); #endif // RADIO_CONTROL_CC2500_PAPARAZZI_H + diff --git a/sw/airborne/modules/radio_control/dummy.c b/sw/airborne/modules/radio_control/dummy.c deleted file mode 100644 index efe582ada9..0000000000 --- a/sw/airborne/modules/radio_control/dummy.c +++ /dev/null @@ -1,24 +0,0 @@ -/* - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#include "modules/radio_control/radio_control.h" - -void radio_control_impl_init(void) { } diff --git a/sw/airborne/modules/radio_control/dummy.h b/sw/airborne/modules/radio_control/dummy.h deleted file mode 100644 index 7b8dea350b..0000000000 --- a/sw/airborne/modules/radio_control/dummy.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * Copyright (C) 2009 Pascal Brisset , - * Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#ifndef RADIO_CONTROL_NULL_H -#define RADIO_CONTROL_NULL_H - -#include "generated/radio.h" - -#define RadioControlEvent(_received_frame_handler) { } - -#endif /* RADIO_CONTROL_NULL_H */ diff --git a/sw/airborne/modules/radio_control/hott.c b/sw/airborne/modules/radio_control/hott.c index 67135797a5..6ae91dc748 100644 --- a/sw/airborne/modules/radio_control/hott.c +++ b/sw/airborne/modules/radio_control/hott.c @@ -14,9 +14,8 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . */ /** @file modules/radio_control/hott.c @@ -26,6 +25,7 @@ #include "modules/radio_control/radio_control.h" #include "modules/radio_control/hott.h" +#include "modules/core/abi.h" #include BOARD_CONFIG @@ -45,9 +45,10 @@ static void send_hott(struct transport_tx *trans, struct link_device *dev) #endif // Init function -void radio_control_impl_init(void) +void hott_init(void) { hott_common_init(&hott, &HOTT_UART_DEV); + radio_control.nb_channel = HOTT_NB_CHANNEL; // Register telemetry message #if PERIODIC_TELEMETRY @@ -58,14 +59,9 @@ void radio_control_impl_init(void) // Decoding event function // Reading from UART -static inline void hott_decode_event(void) +void hott_event(void) { hott_common_decode_event(&hott, &HOTT_UART_DEV); -} - -void radio_control_impl_event(void (* _received_frame_handler)(void)) -{ - hott_decode_event(); if (hott.frame_available) { radio_control.frame_cpt++; radio_control.time_since_last_frame = 0; @@ -74,7 +70,7 @@ void radio_control_impl_event(void (* _received_frame_handler)(void)) } else { radio_control.status = RC_OK; NormalizePpmIIR(hott.pulses, radio_control); - _received_frame_handler(); + AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_HOTT_ID, &radio_control); } hott.frame_available = false; } diff --git a/sw/airborne/modules/radio_control/hott.h b/sw/airborne/modules/radio_control/hott.h index 5208687b30..e0d8af0f03 100644 --- a/sw/airborne/modules/radio_control/hott.h +++ b/sw/airborne/modules/radio_control/hott.h @@ -31,4 +31,15 @@ extern struct SHott hott; +/** + * RC init function. + */ +extern void hott_init(void); + +/** + * RC event function. + */ +extern void hott_event(void); + + #endif /* RC_HOTT_H */ diff --git a/sw/airborne/modules/radio_control/hott_common.c b/sw/airborne/modules/radio_control/hott_common.c index f407969b6f..d76ed0571f 100644 --- a/sw/airborne/modules/radio_control/hott_common.c +++ b/sw/airborne/modules/radio_control/hott_common.c @@ -35,8 +35,8 @@ #define HOTT_STATUS_UNINIT 0 #define HOTT_STATUS_GOT_START 1 -#define HOTT_STATUS_GOT_HEADER1 2 -#define HOTT_STATUS_DATA 3 +#define HOTT_STATUS_GOT_HEADER1 2 +#define HOTT_STATUS_DATA 3 void hott_common_init(struct SHott *hott_p, struct uart_periph *dev) @@ -49,11 +49,10 @@ void hott_common_init(struct SHott *hott_p, struct uart_periph *dev) } #define HOTT_CRC_POLYNOME 0x1021 -/****************************************** -************************************* -* Function Name : CRC16 -* Description : crc calculation, adds a 8 bit unsigned to 16 bit crc -*******************************************************************************/ +/******************************************************************************* +* Function Name : CRC16 +* Description : crc calculation, adds a 8 bit unsigned to 16 bit crc +*******************************************************************************/ static uint16_t hott_CRC16(uint16_t crc, uint8_t value) { uint8_t i; @@ -108,31 +107,30 @@ void hott_common_decode_event(struct SHott *hott_p, struct uart_periph *dev) if (rbyte == HOTT_START_BYTE) { hott_p->status++; hott_p->expected_channels = 0; - hott_p->buffer[hott_p->idx] = rbyte; // store header for crc + hott_p->buffer[hott_p->idx] = rbyte; // store header for crc hott_p->idx++; } break; case HOTT_STATUS_GOT_START: if (rbyte ==0x01 || rbyte ==0x81) { // hott status /* - * SUMD_Header Byte 1 Status + * SUMD_Header Byte 1 Status * 0x01: valid and live SUMD data frame - * 0x81: valid SUMD data frame with transmitter in fail safe condition. + * 0x81: valid SUMD data frame with transmitter in fail safe condition. * Note: - * The SUMD_Data section contains - * valid channel data. The channel data are - * set by transmitter fail safe values. A FBL - * system may replace the transmitter fail safe + * The SUMD_Data section contains + * valid channel data. The channel data are + * set by transmitter fail safe values. A FBL + * system may replace the transmitter fail safe * data by FBL stored values - * - * other values: Values different to 0x01 or 0x81 indicate an invalid SUMD data frame and + * + * other values: Values different to 0x01 or 0x81 indicate an invalid SUMD data frame and * should not be processed by SUMD algorithms * */ hott_p->buffer[hott_p->idx] = rbyte; // store byte for CRC hott_p->idx++; hott_p->status++; - } else - { + } else { hott_p->status=0; // reset } break; diff --git a/sw/airborne/modules/radio_control/hott_common.h b/sw/airborne/modules/radio_control/hott_common.h index 9d11ac8f3c..ac4df1aa38 100644 --- a/sw/airborne/modules/radio_control/hott_common.h +++ b/sw/airborne/modules/radio_control/hott_common.h @@ -37,7 +37,7 @@ /** * Macro to use radio.h file * - * HOTT: 7040..12000..16800 + * HOTT: 7040..12000..16800 * PPM: 880..1500..2100 */ #define RC_PPM_TICKS_OF_USEC(_v) ((_v) * 8 ) // USEC IN HOTT OUT @@ -58,14 +58,7 @@ * input values to RC channels. */ #define HOTT_NB_CHANNEL 32 -#define HOTT_BUF_LENGTH (HOTT_NB_CHANNEL*2+3+2) // 2 bytes per chennel 3 bytes header, 2 bytes CRC - -/** - * Default number of channels to actually use. - */ -#ifndef RADIO_CONTROL_NB_CHANNEL -#define RADIO_CONTROL_NB_CHANNEL HOTT_NB_CHANNEL -#endif +#define HOTT_BUF_LENGTH (HOTT_NB_CHANNEL*2+3+2) // 2 bytes per chennel 3 bytes header, 2 bytes CRC #if RADIO_CONTROL_NB_CHANNEL > HOTT_NB_CHANNEL #error "RADIO_CONTROL_NB_CHANNEL mustn't be higher than 32." @@ -94,16 +87,4 @@ void hott_common_init(struct SHott *hott, struct uart_periph *dev); */ void hott_common_decode_event(struct SHott *hott, struct uart_periph *dev); - -/** - * RC event function with handler callback. - */ -extern void radio_control_impl_event(void (* _received_frame_handler)(void)); - -/** - * Event macro with handler callback - */ -#define RadioControlEvent(_received_frame_handler) radio_control_impl_event(_received_frame_handler) - - -#endif /* RC_HOTT_H */ +#endif /* RC_HOTT_COMMON_H */ diff --git a/sw/airborne/modules/radio_control/ppm.c b/sw/airborne/modules/radio_control/ppm.c index 53e66065fb..23e64b23c2 100644 --- a/sw/airborne/modules/radio_control/ppm.c +++ b/sw/airborne/modules/radio_control/ppm.c @@ -27,6 +27,11 @@ #include "modules/radio_control/radio_control.h" #include "modules/radio_control/ppm.h" +#include "modules/core/abi.h" + +#if RADIO_CONTROL_NB_CHANNEL < RADIO_CTL_NB +#error "RADIO_CONTROL_NB_CHANNEL mustn't be lower than number of channels in radio file." +#endif uint16_t ppm_pulses[RADIO_CTL_NB]; volatile bool ppm_frame_available; @@ -63,12 +68,13 @@ static void send_ppm(struct transport_tx *trans, struct link_device *dev) } #endif -void radio_control_impl_init(void) +void ppm_init(void) { ppm_frame_available = false; ppm_last_pulse_time = 0; ppm_cur_pulse = RADIO_CTL_NB; ppm_data_valid = false; + radio_control.nb_channel = RADIO_CTL_NB; ppm_arch_init(); @@ -77,7 +83,7 @@ void radio_control_impl_init(void) #endif } -void radio_control_impl_event(void (* _received_frame_handler)(void)) +void ppm_event(void) { if (ppm_frame_available) { radio_control.frame_cpt++; @@ -87,7 +93,7 @@ void radio_control_impl_event(void (* _received_frame_handler)(void)) } else { radio_control.status = RC_OK; NormalizePpmIIR(ppm_pulses, radio_control); - _received_frame_handler(); + AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_PPM_ID, &radio_control); } ppm_frame_available = false; } diff --git a/sw/airborne/modules/radio_control/ppm.h b/sw/airborne/modules/radio_control/ppm.h index 071f142d53..7036ab609a 100644 --- a/sw/airborne/modules/radio_control/ppm.h +++ b/sw/airborne/modules/radio_control/ppm.h @@ -40,17 +40,6 @@ extern void ppm_arch_init(void); */ #include "generated/radio.h" -/** - * Default number of channels to actually use. - */ -#ifndef RADIO_CONTROL_NB_CHANNEL -#define RADIO_CONTROL_NB_CHANNEL RADIO_CTL_NB -#endif - -#if RADIO_CONTROL_NB_CHANNEL > RADIO_CTL_NB -#error "RADIO_CONTROL_NB_CHANNEL mustn't be higher than number of channels in radio file." -#endif - /** * ppm pulse type : futaba is falling edge clocked whereas JR is rising edge */ @@ -61,13 +50,15 @@ extern uint16_t ppm_pulses[RADIO_CTL_NB]; extern volatile bool ppm_frame_available; /** - * RC event function with handler callback. + * RC init function. + */ +extern void ppm_init(void); + +/** + * RC event function. * PPM frames are normalized using the IIR filter. */ -extern void radio_control_impl_event(void (* _received_frame_handler)(void)); - -#define RadioControlEvent(_received_frame_handler) radio_control_impl_event(_received_frame_handler) - +extern void ppm_event(void); /** * Decode a PPM frame from global timer value. diff --git a/sw/airborne/modules/radio_control/radio_control.c b/sw/airborne/modules/radio_control/radio_control.c index 43a3889cd4..ea08e1c3d0 100644 --- a/sw/airborne/modules/radio_control/radio_control.c +++ b/sw/airborne/modules/radio_control/radio_control.c @@ -26,9 +26,21 @@ */ #include "modules/radio_control/radio_control.h" +#include "led.h" + +//PRINT_CONFIG_VAR(RADIO_CONTROL_NB_CHANNEL) struct RadioControl radio_control; +#if PERIODIC_TELEMETRY +#include "modules/datalink/telemetry.h" + +static void send_rc(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_RC(trans, dev, AC_ID, radio_control.nb_channel, radio_control.values); +} +#endif + void radio_control_init(void) { uint8_t i; @@ -40,7 +52,11 @@ void radio_control_init(void) radio_control.radio_ok_cpt = 0; radio_control.frame_rate = 0; radio_control.frame_cpt = 0; - radio_control_impl_init(); + radio_control.nb_channel = RADIO_CONTROL_NB_CHANNEL; // can be changed by selected RC module + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc); +#endif } void radio_control_periodic_task(void) diff --git a/sw/airborne/modules/radio_control/radio_control.h b/sw/airborne/modules/radio_control/radio_control.h index 9aed364b07..c63cdc84a1 100644 --- a/sw/airborne/modules/radio_control/radio_control.h +++ b/sw/airborne/modules/radio_control/radio_control.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2006 Pascal Brisset, Antoine Drouin + * 2021 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,30 +15,22 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ +/** + * @file modules/radio_control/radio_control.h + * Generic interface for radio control modules + */ + #ifndef RADIO_CONTROL_H #define RADIO_CONTROL_H -#include "led.h" #include "generated/airframe.h" #include "paparazzi.h" -/* underlying hardware, also include if RADIO_CONTROL is not defined for ap in dual mcu case */ -#include RADIO_CONTROL_TYPE_H - -/* RADIO_CONTROL_NB_CHANNEL needs to be defined to suitable default the implementation. - * If not all available channels are needed, can be overridden in airframe file. - */ - -#if defined RADIO_CONTROL -/* must be defined by underlying hardware */ -extern void radio_control_impl_init(void); - /* timeouts - for now assumes 60Hz periodic */ #ifndef RC_AVG_PERIOD #define RC_AVG_PERIOD 8 /* TODO remove if IIR filter is used */ @@ -57,8 +50,12 @@ extern void radio_control_impl_init(void); #define RC_LOST 1 #define RC_REALLY_LOST 2 -/* macro that can be used in the command laws */ -#define RCValue(_x) radio_control.values[_x] +// if not redefined, use default number of channels +// value should be large enough for all implementation by default +#ifndef RADIO_CONTROL_NB_CHANNEL +#define RADIO_CONTROL_NB_CHANNEL 32 +#endif + struct RadioControl { uint8_t status; @@ -66,18 +63,44 @@ struct RadioControl { uint8_t radio_ok_cpt; uint8_t frame_rate; uint8_t frame_cpt; + uint8_t nb_channel; pprz_t values[RADIO_CONTROL_NB_CHANNEL]; }; extern struct RadioControl radio_control; -#define RadioControlValues(_chan) radio_control.values[_chan] ///< For easy access in command_laws +// For easy access in command_laws +#define RadioControlValues(_chan) radio_control.values[_chan] + +// Test is radio is reall lost +#define RadioControlIsLost() (radio_control.status == RC_REALLY_LOST) + +/** Set a radio control channel value + * @param idx rc channel index + * @param value new value + */ +static inline void radio_control_set(uint8_t idx, pprz_t value) +{ + if (idx < radio_control.nb_channel) { + // Bound value ??? + radio_control.values[idx] = value; + } +} + +/** Get a radio control channel value + * @param idx rc channel index + * @return current value, 0 if index is invalid + */ +static inline pprz_t radio_control_get(uint8_t idx) +{ + if (idx < radio_control.nb_channel) { + return radio_control.values[idx]; + } + return 0; +} + + extern void radio_control_init(void); extern void radio_control_periodic_task(void); -// Event implemented in radio_control/*.h - - -#endif /* RADIO_CONTROL */ - #endif /* RADIO_CONTROL_H */ diff --git a/sw/airborne/modules/radio_control/rc_datalink.c b/sw/airborne/modules/radio_control/rc_datalink.c index d3cc64ed09..8bff9ee43d 100644 --- a/sw/airborne/modules/radio_control/rc_datalink.c +++ b/sw/airborne/modules/radio_control/rc_datalink.c @@ -26,13 +26,15 @@ #include "modules/radio_control/rc_datalink.h" #include "modules/radio_control/radio_control.h" +#include "modules/core/abi.h" int8_t rc_dl_values[ RC_DL_NB_CHANNEL ]; volatile bool rc_dl_frame_available; -void radio_control_impl_init(void) +void rc_datalink_init(void) { + radio_control.nb_channel = RC_DL_NB_CHANNEL; rc_dl_frame_available = false; } @@ -86,7 +88,7 @@ static void rc_datalink_normalize(int8_t *in, int16_t *out) Bound(out[RADIO_MODE], MIN_PPRZ, MAX_PPRZ); } -void radio_control_impl_event(void (* _received_frame_handler)(void)) +void rc_datalink_event(void) { if (rc_dl_frame_available) { radio_control.frame_cpt++; @@ -94,7 +96,7 @@ void radio_control_impl_event(void (* _received_frame_handler)(void)) radio_control.radio_ok_cpt = 0; radio_control.status = RC_OK; rc_datalink_normalize(rc_dl_values, radio_control.values); - _received_frame_handler(); + AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_DATALINK_ID, &radio_control); rc_dl_frame_available = false; } } diff --git a/sw/airborne/modules/radio_control/rc_datalink.h b/sw/airborne/modules/radio_control/rc_datalink.h index 6bbb8a567a..ff047cdecd 100644 --- a/sw/airborne/modules/radio_control/rc_datalink.h +++ b/sw/airborne/modules/radio_control/rc_datalink.h @@ -30,7 +30,6 @@ #include "std.h" #define RC_DL_NB_CHANNEL 5 -#define RADIO_CONTROL_NB_CHANNEL RC_DL_NB_CHANNEL /** * Redefining RADIO_* @@ -65,13 +64,14 @@ extern void parse_rc_4ch_datalink( int8_t yaw); /** - * RC event function with handler callback. + * RC init function. */ -extern void radio_control_impl_event(void (* _received_frame_handler)(void)); +extern void rc_datalink_init(void); /** - * Event macro with handler callback + * RC event function. */ -#define RadioControlEvent(_received_frame_handler) radio_control_impl_event(_received_frame_handler) +extern void rc_datalink_event(void); #endif /* RC_DATALINK_H */ + diff --git a/sw/airborne/modules/radio_control/rc_intermcu.c b/sw/airborne/modules/radio_control/rc_intermcu.c new file mode 100644 index 0000000000..4f1e581e42 --- /dev/null +++ b/sw/airborne/modules/radio_control/rc_intermcu.c @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2022 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/radio_control/rc_intermcu.c + * + * Radio control input via intermcu. + */ + +#include "modules/radio_control/rc_intermcu.h" +#include "modules/radio_control/radio_control.h" +#include "pprzlink/intermcu_msg.h" +#include "modules/core/abi.h" + +struct _rc_intermcu { + uint16_t values[RC_IMCU_NB_CHANNEL]; + uint8_t status; + uint8_t frame_rate; +}; + +static struct _rc_intermcu rc_intermcu; + +void rc_intermcu_init(void) +{ + radio_control.nb_channel = RC_IMCU_NB_CHANNEL; + rc_intermcu.status = RC_REALLY_LOST; + rc_intermcu.frame_rate = 0; + for (int i = 0; i < RC_IMCU_NB_CHANNEL; i++) { + rc_intermcu.values[i] = 0; + } +} + + +void rc_intermcu_parse_msg(uint8_t *buf) +{ + uint8_t size = DL_IMCU_RADIO_COMMANDS_values_length(buf); + //rc_intermcu.status = DL_IMCU_RADIO_COMMANDS_status(imcu_msg_buf); TODO change status on FBW side to have RC status here + for (uint8_t i = 0; i < size; i++) { + rc_intermcu.values[i] = DL_IMCU_RADIO_COMMANDS_values(buf)[i]; + radio_control.values[i] = rc_intermcu.values[i]; // for now a simple copy + } + + radio_control.frame_cpt++; + radio_control.time_since_last_frame = 0; + radio_control.radio_ok_cpt = 0; + radio_control.status = RC_OK; + AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_INTERMCU_ID, &radio_control); +} + +void rc_intermcu_parse_fbw_status(uint8_t *buf) +{ + // update status and frame rate, send message for mode update + rc_intermcu.status = DL_IMCU_FBW_STATUS_rc_status(buf); + rc_intermcu.frame_rate = DL_IMCU_FBW_STATUS_frame_rate(buf); + AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_INTERMCU_ID, &radio_control); +} + diff --git a/sw/airborne/modules/radio_control/rc_intermcu.h b/sw/airborne/modules/radio_control/rc_intermcu.h new file mode 100644 index 0000000000..774946e5b1 --- /dev/null +++ b/sw/airborne/modules/radio_control/rc_intermcu.h @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2022 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/radio_control/rc_intermcu.h + * + * Radio control input via intermcu. + */ + +#ifndef RC_INTERMCU_H +#define RC_INTERMCU_H + +#include "std.h" + +/* We need radio defines for the Autopilot + * index are matching intermcu_fbw.h + */ +#define RADIO_THROTTLE 0 +#define RADIO_ROLL 1 +#define RADIO_PITCH 2 +#define RADIO_YAW 3 +#define RADIO_MODE 4 +#define RADIO_KILL_SWITCH 5 +#define RADIO_AUX1 5 // be careful with ID 5 +#define RADIO_AUX2 6 +#define RADIO_AUX3 7 + +#define RC_IMCU_NB_CHANNEL 8 + + +/** + * RC init function. + */ +extern void rc_intermcu_init(void); + +/** + * Decode intermcu message to get rc values + * and FBW status for RC status and frame rate TODO make a single message + */ +extern void rc_intermcu_parse_msg(uint8_t *buf); +extern void rc_intermcu_parse_fbw_status(uint8_t *buf); + +#endif /* RC_INTERMCU_H */ + diff --git a/sw/airborne/modules/radio_control/sbus.c b/sw/airborne/modules/radio_control/sbus.c index 1702cedab8..77a80973ea 100644 --- a/sw/airborne/modules/radio_control/sbus.c +++ b/sw/airborne/modules/radio_control/sbus.c @@ -26,6 +26,7 @@ #include "modules/radio_control/radio_control.h" #include "modules/radio_control/sbus.h" +#include "modules/core/abi.h" #include BOARD_CONFIG #ifndef RC_POLARITY_GPIO_PORT @@ -51,9 +52,10 @@ static void send_sbus(struct transport_tx *trans, struct link_device *dev) #endif // Init function -void radio_control_impl_init(void) +void sbus_init(void) { sbus_common_init(&sbus, &SBUS_UART_DEV, RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); + radio_control.nb_channel = SBUS_NB_CHANNEL; // Register telemetry message #if PERIODIC_TELEMETRY @@ -69,7 +71,7 @@ static inline void sbus_decode_event(void) sbus_common_decode_event(&sbus, &SBUS_UART_DEV); } -void radio_control_impl_event(void (* _received_frame_handler)(void)) +void sbus_event(void) { sbus_decode_event(); @@ -89,7 +91,7 @@ void radio_control_impl_event(void (* _received_frame_handler)(void)) } else { radio_control.status = RC_OK; NormalizePpmIIR(sbus.pulses, radio_control); - _received_frame_handler(); + AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_SBUS_ID, &radio_control); } } sbus.frame_available = false; diff --git a/sw/airborne/modules/radio_control/sbus.h b/sw/airborne/modules/radio_control/sbus.h index 9698414d8e..ee2e85d02f 100644 --- a/sw/airborne/modules/radio_control/sbus.h +++ b/sw/airborne/modules/radio_control/sbus.h @@ -31,4 +31,14 @@ extern struct Sbus sbus; +/** + * RC init function. + */ +extern void sbus_init(void); + +/** + * RC event function. + */ +extern void sbus_event(void); + #endif /* RC_SBUS_H */ diff --git a/sw/airborne/modules/radio_control/sbus_common.c b/sw/airborne/modules/radio_control/sbus_common.c index f61996d888..8846e9b70d 100644 --- a/sw/airborne/modules/radio_control/sbus_common.c +++ b/sw/airborne/modules/radio_control/sbus_common.c @@ -30,6 +30,10 @@ #include #include +#if RADIO_CONTROL_NB_CHANNEL < SBUS_NB_CHANNEL +#error "RADIO_CONTROL_NB_CHANNEL mustn't be lower than 16." +#endif + /* * SBUS protocol and state machine status */ diff --git a/sw/airborne/modules/radio_control/sbus_common.h b/sw/airborne/modules/radio_control/sbus_common.h index 4793a00135..8d12db1019 100644 --- a/sw/airborne/modules/radio_control/sbus_common.h +++ b/sw/airborne/modules/radio_control/sbus_common.h @@ -62,17 +62,6 @@ #define SBUS_BUF_LENGTH 24 #define SBUS_NB_CHANNEL 16 -/** - * Default number of channels to actually use. - */ -#ifndef RADIO_CONTROL_NB_CHANNEL -#define RADIO_CONTROL_NB_CHANNEL SBUS_NB_CHANNEL -#endif - -#if RADIO_CONTROL_NB_CHANNEL > SBUS_NB_CHANNEL -#error "RADIO_CONTROL_NB_CHANNEL mustn't be higher than 16." -#endif - /** * SBUS structure */ @@ -99,16 +88,5 @@ void sbus_common_init(struct Sbus *sbus, struct uart_periph *dev, */ void sbus_common_decode_event(struct Sbus *sbus, struct uart_periph *dev); +#endif /* RC_SBUS_COMMON_H */ -/** - * RC event function with handler callback. - */ -extern void radio_control_impl_event(void (* _received_frame_handler)(void)); - -/** - * Event macro with handler callback - */ -#define RadioControlEvent(_received_frame_handler) radio_control_impl_event(_received_frame_handler) - - -#endif /* RC_SBUS_H */ diff --git a/sw/airborne/modules/radio_control/sbus_dual.c b/sw/airborne/modules/radio_control/sbus_dual.c index eb4617ce9f..a83548502d 100644 --- a/sw/airborne/modules/radio_control/sbus_dual.c +++ b/sw/airborne/modules/radio_control/sbus_dual.c @@ -26,6 +26,7 @@ #include "modules/radio_control/radio_control.h" #include "modules/radio_control/sbus_dual.h" +#include "modules/core/abi.h" #include BOARD_CONFIG #include "mcu_periph/uart.h" #include @@ -60,10 +61,11 @@ static void send_sbus(struct transport_tx *trans, struct link_device *dev) #endif // Init function -void radio_control_impl_init(void) +void sbus_dual_init(void) { sbus_common_init(&sbus1, &SBUS1_UART_DEV, RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); sbus_common_init(&sbus2, &SBUS2_UART_DEV, RC2_POLARITY_GPIO_PORT, RC2_POLARITY_GPIO_PIN); + radio_control.nb_channel = SBUS_NB_CHANNEL; // Register telemetry message #if PERIODIC_TELEMETRY @@ -77,7 +79,7 @@ static inline void sbus_dual_decode_event(void) sbus_common_decode_event(&sbus2, &SBUS2_UART_DEV); } -void radio_control_impl_event(void (* _received_frame_handler)(void)) +void sbus_dual_event(void) { sbus_dual_decode_event(); if (sbus2.frame_available) { @@ -88,7 +90,7 @@ void radio_control_impl_event(void (* _received_frame_handler)(void)) } else { radio_control.status = RC_OK; NormalizePpmIIR(sbus2.pulses, radio_control); - _received_frame_handler(); + AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_SBUS_DUAL_ID, &radio_control); } sbus2.frame_available = false; } @@ -100,7 +102,7 @@ void radio_control_impl_event(void (* _received_frame_handler)(void)) } else { radio_control.status = RC_OK; NormalizePpmIIR(sbus1.pulses, radio_control); - _received_frame_handler(); + AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_SBUS_DUAL_ID, &radio_control); } sbus1.frame_available = false; } diff --git a/sw/airborne/modules/radio_control/sbus_dual.h b/sw/airborne/modules/radio_control/sbus_dual.h index 759fd03017..b7b507956d 100644 --- a/sw/airborne/modules/radio_control/sbus_dual.h +++ b/sw/airborne/modules/radio_control/sbus_dual.h @@ -31,4 +31,15 @@ extern struct Sbus sbus1, sbus2; -#endif /* RC_SBUS_H */ +/** + * RC init function. + */ +extern void sbus_dual_init(void); + +/** + * RC event function. + */ +extern void sbus_dual_event(void); + + +#endif /* RC_SBUS_DUAL_H */ diff --git a/sw/airborne/modules/radio_control/spektrum.c b/sw/airborne/modules/radio_control/spektrum.c index 965e95bcc2..f968d685cc 100644 --- a/sw/airborne/modules/radio_control/spektrum.c +++ b/sw/airborne/modules/radio_control/spektrum.c @@ -21,8 +21,6 @@ * . */ -#include "modules/radio_control/spektrum.h" - /** * @file modules/radio_control/spektrum.c * @@ -31,11 +29,17 @@ */ #include "std.h" +#include "modules/radio_control/spektrum.h" #include "modules/radio_control/radio_control.h" +#include "modules/core/abi.h" #include "mcu_periph/uart.h" #include "mcu_periph/gpio.h" #include "mcu_periph/sys_time.h" +#if RADIO_CONTROL_NB_CHANNEL < SPEKTRUM_NB_CHANNEL +#warning "RADIO_CONTROL_NB_CHANNEL mustn't be lower than 14. X-Plus channel expansion is not (yet) usable" +#endif + /* Check if primary receiver is defined */ #ifndef SPEKTRUM_PRIMARY_UART #error "You must at least define the primary Spektrum satellite receiver." @@ -158,12 +162,13 @@ void spektrum_try_bind(void) /** Main Radio initialization */ -void radio_control_impl_init(void) +void spektrum_init(void) { - for (uint8_t i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) { + for (uint8_t i = 0; i < SPEKTRUM_NB_CHANNEL; i++) { spektrum.signs[i] = spektrum_signs[i]; } + radio_control.nb_channel = SPEKTRUM_NB_CHANNEL; // Set polarity to normal on boards that can change this #ifdef RC_POLARITY_GPIO_PORT @@ -279,7 +284,7 @@ static void spektrum_uart_check(struct uart_periph *dev, struct spektrum_sat_t * } /** Checks if there is one valid satellite and sets the radio_control structure */ -void spektrum_event(void (*frame_handler)(void)) +void spektrum_event(void) { spektrum_uart_check(&SPEKTRUM_PRIMARY_UART, &spektrum.satellites[0]); @@ -313,13 +318,13 @@ void spektrum_event(void (*frame_handler)(void)) radio_control.status = RC_OK; // Copy the radio control channels - for (uint8_t i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) { + for (uint8_t i = 0; i < radio_control.nb_channel; i++) { radio_control.values[i] = spektrum.satellites[sat_id].values[i] * spektrum.signs[i]; Bound(radio_control.values[i], -MAX_PPRZ, MAX_PPRZ); } // We got a valid frame so execute the frame handler - (*frame_handler)(); + AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_SPEKTRUM_ID, &radio_control); } } diff --git a/sw/airborne/modules/radio_control/spektrum.h b/sw/airborne/modules/radio_control/spektrum.h index 3d4a5d7570..b6ac1b8597 100644 --- a/sw/airborne/modules/radio_control/spektrum.h +++ b/sw/airborne/modules/radio_control/spektrum.h @@ -44,9 +44,6 @@ #define SPEKTRUM_MAX_CHANNELS (SPEKTRUM_CHANNELS_PER_FRAME * SPEKTRUM_MAX_FRAMES) #define SPEKTRUM_MIN_FRAME_SPACE 7 ///< Minum amount of time between frames (7ms), in fact either 11 or 22 ms -/* Set the event function to the correct */ -#define RadioControlEvent(_received_frame_handler) spektrum_event(_received_frame_handler) - /* Per satellite we keep track of data */ struct spektrum_sat_t { bool valid; ///< True when we received a packet else false @@ -61,12 +58,13 @@ struct spektrum_sat_t { struct spektrum_t { bool valid; ///< True when we received a packet else false uint8_t tx_type; ///< Transmitter type encoded (see wiki) - int8_t signs[RADIO_CONTROL_NB_CHANNEL]; ///< Signs for the RC channels + int8_t signs[SPEKTRUM_NB_CHANNEL]; ///< Signs for the RC channels struct spektrum_sat_t satellites[SPEKTRUM_SATELLITES_NB]; ///< All the satellites connected }; /* External functions */ -extern void spektrum_event(void (*_received_frame_handler)(void)); +extern void spektrum_init(void); +extern void spektrum_event(void); extern void spektrum_try_bind(void); #endif /* RADIO_CONTROL_SPEKTRUM_H */ diff --git a/sw/airborne/modules/radio_control/spektrum_radio.h b/sw/airborne/modules/radio_control/spektrum_radio.h index ebef0c23fb..503e5ef8f8 100644 --- a/sw/airborne/modules/radio_control/spektrum_radio.h +++ b/sw/airborne/modules/radio_control/spektrum_radio.h @@ -29,13 +29,7 @@ #define RADIO_CONTROL_SPEKTRUM_RADIO_H /* Amount of Spektrum channels */ -#ifndef RADIO_CONTROL_NB_CHANNEL -#define RADIO_CONTROL_NB_CHANNEL 14 -#endif - -#if RADIO_CONTROL_NB_CHANNEL > 14 -#error "RADIO_CONTROL_NB_CHANNEL mustn't be higher than 14. X-Plus channel expansion is not (yet) usable" -#endif +#define SPEKTRUM_NB_CHANNEL 14 /* Default channel assignments */ #ifndef RADIO_THROTTLE diff --git a/sw/airborne/modules/radio_control/superbitrf_rc.c b/sw/airborne/modules/radio_control/superbitrf_rc.c index c24ebced8e..f57a5974a5 100644 --- a/sw/airborne/modules/radio_control/superbitrf_rc.c +++ b/sw/airborne/modules/radio_control/superbitrf_rc.c @@ -25,13 +25,19 @@ #include "superbitrf_rc.h" #include "modules/radio_control/radio_control.h" +#include "modules/core/abi.h" + +#if RADIO_CONTROL_NB_CHANNEL < SUPERBITRF_RC_NB_CHANNEL +#error "RADIO_CONTROL_NB_CHANNEL mustn't be lower than 14. X-Plus channel expansion is not (yet) usable" +#endif /** * Initialization */ -void radio_control_impl_init(void) +void superbitrf_rc_init(void) { superbitrf_init(); + radio_control.nb_channel = SUPERBITRF_RC_NB_CHANNEL; } /** normalize superbitrf rc_values to radio values */ @@ -49,7 +55,7 @@ static void superbitrf_rc_normalize(int16_t *in, int16_t *out, uint8_t count) } } -void radio_control_impl_event(void (* _received_frame_handler)(void)) +void superbitrf_rc_event(void) { cyrf6936_event(&superbitrf.cyrf6936); superbitrf_event(); @@ -60,7 +66,7 @@ void radio_control_impl_event(void (* _received_frame_handler)(void)) radio_control.status = RC_OK; superbitrf_rc_normalize(superbitrf.rc_values, radio_control.values, superbitrf.num_channels); - _received_frame_handler(); + AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_SUPERBITRF_RC_ID, &radio_control); superbitrf.rc_frame_available = false; } } diff --git a/sw/airborne/modules/radio_control/superbitrf_rc.h b/sw/airborne/modules/radio_control/superbitrf_rc.h index 8a2db4871c..77cebb6244 100644 --- a/sw/airborne/modules/radio_control/superbitrf_rc.h +++ b/sw/airborne/modules/radio_control/superbitrf_rc.h @@ -29,13 +29,7 @@ #include "modules/datalink/superbitrf.h" /* Theoretically you could have 14 channel over DSM2/DSMX */ -#ifndef RADIO_CONTROL_NB_CHANNEL -#define RADIO_CONTROL_NB_CHANNEL 14 -#endif - -#if RADIO_CONTROL_NB_CHANNEL > 14 -#error "RADIO_CONTROL_NB_CHANNEL mustn't be higher than 14. X-Plus channel expansion is not (yet) usable" -#endif +#define SUPERBITRF_RC_NB_CHANNEL 14 /* Default channel assignments */ #ifndef RADIO_THROTTLE @@ -90,11 +84,13 @@ #endif /** - * RC event function with handler callback. + * RC init function. */ -extern void radio_control_impl_event(void (* _received_frame_handler)(void)); +extern void superbitrf_rc_init(void); -/* The radio control event handler */ -#define RadioControlEvent(_received_frame_handler) radio_control_impl_event(_received_frame_handler) +/** + * RC event function. + */ +extern void superbitrf_rc_event(void); #endif /* RADIO_CONTROL_SUPERBITRF_RC_H */ diff --git a/sw/airborne/modules/switching/auto1_commands.c b/sw/airborne/modules/switching/auto1_commands.c index fe12f4f298..09cb997467 100644 --- a/sw/airborne/modules/switching/auto1_commands.c +++ b/sw/airborne/modules/switching/auto1_commands.c @@ -23,7 +23,7 @@ #include "auto1_commands.h" #include "generated/airframe.h" #include "autopilot.h" -#include "modules/intermcu/inter_mcu.h" +#include "modules/core/commands.h" void periodic_auto1_commands(void) { @@ -33,67 +33,67 @@ void periodic_auto1_commands(void) #ifndef RADIO_GEAR #error auto1_commands COMMAND_GEAR needs RADIO_GEAR channel #endif - imcu_set_command(COMMAND_GEAR, imcu_get_radio(RADIO_GEAR)); + command_set(COMMAND_GEAR, radio_control_get(RADIO_GEAR)); #endif #ifdef COMMAND_FLAP #ifndef RADIO_FLAP #error auto1_commands COMMAND_FLAP needs RADIO_FLAP channel #endif - imcu_set_command(COMMAND_FLAP, imcu_get_radio(RADIO_FLAP)); + command_set(COMMAND_FLAP, radio_control_get(RADIO_FLAP)); #endif #ifdef COMMAND_AUX1 #ifndef RADIO_AUX1 #error auto1_commands COMMAND_AUX1 needs RADIO_AUX1 channel #endif - imcu_set_command(COMMAND_AUX1, imcu_get_radio(RADIO_AUX1)); + command_set(COMMAND_AUX1, radio_control_get(RADIO_AUX1)); #endif #ifdef COMMAND_AUX2 #ifndef RADIO_AUX2 #error auto1_commands COMMAND_AUX2 needs RADIO_AUX2 channel #endif - imcu_set_command(COMMAND_AUX2, imcu_get_radio(RADIO_AUX2)); + command_set(COMMAND_AUX2, radio_control_get(RADIO_AUX2)); #endif #ifdef COMMAND_AUX3 #ifndef RADIO_AUX3 #error auto1_commands COMMAND_AUX1 needs RADIO_AUX3 channel #endif - imcu_set_command(COMMAND_AUX3, imcu_get_radio(RADIO_AUX3)); + command_set(COMMAND_AUX3, radio_control_get(RADIO_AUX3)); #endif #ifdef COMMAND_AUX4 #ifndef RADIO_AUX4 #error auto1_commands COMMAND_AUX4 needs RADIO_AUX4 channel #endif - imcu_set_command(COMMAND_AUX4, imcu_get_radio(RADIO_AUX4)); + command_set(COMMAND_AUX4, radio_control_get(RADIO_AUX4)); #endif #ifdef COMMAND_AUX5 #ifndef RADIO_AUX5 #error auto1_commands COMMAND_AUX5 needs RADIO_AUX5 channel #endif - imcu_set_command(COMMAND_AUX5, imcu_get_radio(RADIO_AUX5)); + command_set(COMMAND_AUX5, radio_control_get(RADIO_AUX5)); #endif #ifdef COMMAND_AUX6 #ifndef RADIO_AUX6 #error auto1_commands COMMAND_AUX6 needs RADIO_AUX6 channel #endif - imcu_set_command(COMMAND_AUX6, imcu_get_radio(RADIO_AUX6)); + command_set(COMMAND_AUX6, radio_control_get(RADIO_AUX6)); #endif #ifdef COMMAND_AUX7 #ifndef RADIO_AUX7 #error auto1_commands COMMAND_AUX7 needs RADIO_AUX7 channel #endif - imcu_set_command(COMMAND_AUX7, imcu_get_radio(RADIO_AUX7)); + command_set(COMMAND_AUX7, radio_control_get(RADIO_AUX7)); #endif #ifdef COMMAND_BRAKE #ifndef RADIO_BRAKE #error auto1_commands COMMAND_BRAKE needs RADIO_BRAKE channel #endif - imcu_set_command(COMMAND_BRAKE, imcu_get_radio(RADIO_BRAKE)); + command_set(COMMAND_BRAKE, radio_control_get(RADIO_BRAKE)); #endif #ifdef COMMAND_HATCH #ifndef RADIO_HATCH #error auto1_commands COMMAND_HATCH needs RADIO_HATCH channel #endif - imcu_set_command(COMMAND_HATCH, imcu_get_radio(RADIO_HATCH)); + command_set(COMMAND_HATCH, radio_control_get(RADIO_HATCH)); #endif } } diff --git a/sw/airborne/modules/telemetry/telemetry_intermcu.h b/sw/airborne/modules/telemetry/telemetry_intermcu.h deleted file mode 100644 index 7ab04a47c5..0000000000 --- a/sw/airborne/modules/telemetry/telemetry_intermcu.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * Copyright (C) 2016 Freek van Tienen - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** @file modules/telemetry/telemetry_intermcu.h - * @brief Telemetry through InterMCU - */ - -#ifndef TELEMETRY_INTERMCU_H -#define TELEMETRY_INTERMCU_H - -#include "std.h" - -/* External functions */ -extern void telemetry_intermcu_init(void); -extern void telemetry_intermcu_periodic(void); -extern void telemetry_intermcu_event(void); -extern void telemetry_intermcu_on_msg(uint8_t* msg, uint8_t size); - -#endif /* TELEMETRY_INTERMCU_H */ diff --git a/sw/airborne/modules/telemetry/telemetry_intermcu_ap.c b/sw/airborne/modules/telemetry/telemetry_intermcu_ap.c deleted file mode 100644 index d62459f2cc..0000000000 --- a/sw/airborne/modules/telemetry/telemetry_intermcu_ap.c +++ /dev/null @@ -1,106 +0,0 @@ -/* - * Copyright (C) 2016 Freek van Tienen - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** @file modules/telemetry/telemetry_intermcu_ap.c - * @brief Telemetry through InterMCU - */ - -#define PERIODIC_C_INTERMCU -#include "telemetry_intermcu.h" -#include "telemetry_intermcu_ap.h" -#include "modules/intermcu/intermcu.h" -#include "pprzlink/intermcu_msg.h" -#include "pprzlink/short_transport.h" -#include "generated/periodic_telemetry.h" -#include "modules/datalink/telemetry.h" -#include "modules/datalink/datalink.h" - -/* Telemetry InterMCU throughput */ -struct telemetry_intermcu_t telemetry_intermcu; - -/* Static functions */ -static int telemetry_intermcu_check_free_space(struct telemetry_intermcu_t *p, long *fd __attribute__((unused)), uint16_t len); -static void telemetry_intermcu_put_byte(struct telemetry_intermcu_t *p, long fd __attribute__((unused)), uint8_t data); -static void telemetry_intermcu_put_buffer(struct telemetry_intermcu_t *p, long fd, uint8_t *data, uint16_t len); -static void telemetry_intermcu_send_message(struct telemetry_intermcu_t *p, long fd __attribute__((unused))); - -/* InterMCU initialization */ -void telemetry_intermcu_init(void) -{ - // Initialize transport structure - short_transport_init(&telemetry_intermcu.trans); - - // Configure the device - telemetry_intermcu.dev.check_free_space = (check_free_space_t)telemetry_intermcu_check_free_space; - telemetry_intermcu.dev.put_byte = (put_byte_t)telemetry_intermcu_put_byte; - telemetry_intermcu.dev.put_buffer = (put_buffer_t)telemetry_intermcu_put_buffer; - telemetry_intermcu.dev.send_message = (send_message_t)telemetry_intermcu_send_message; - telemetry_intermcu.dev.periph = (void *)&telemetry_intermcu; -} - -/* InterMCU periodic handling of telemetry */ -void telemetry_intermcu_periodic(void) -{ - periodic_telemetry_send_InterMCU(DefaultPeriodic, &telemetry_intermcu.trans.trans_tx, &telemetry_intermcu.dev); -} - -/* InterMCU event handling of telemetry */ -void telemetry_intermcu_event(void) -{ - -} - -void telemetry_intermcu_on_msg(uint8_t* msg, uint8_t size __attribute__((unused))) -{ - datalink_time = 0; - datalink_nb_msgs++; - dl_parse_msg(&telemetry_intermcu.dev, &telemetry_intermcu.trans.trans_tx, msg); -} - -static int telemetry_intermcu_check_free_space(struct telemetry_intermcu_t *p, long *fd __attribute__((unused)), uint16_t len) -{ - int available = TELEMERTY_INTERMCU_MSG_SIZE - p->buf_idx; - return available >= len ? available : 0; -} - -static void telemetry_intermcu_put_byte(struct telemetry_intermcu_t *p, long fd __attribute__((unused)), uint8_t data) -{ - if(p->buf_idx >= (TELEMERTY_INTERMCU_MSG_SIZE - 1)) - return; - - p->buf[p->buf_idx++] = data; -} - -static void telemetry_intermcu_put_buffer(struct telemetry_intermcu_t *p, long fd __attribute__((unused)), uint8_t *data, uint16_t len) -{ - int i; - for (i = 0; i < len; i++) { - p->buf[p->buf_idx++] = data[i]; - } -} - -static void telemetry_intermcu_send_message(struct telemetry_intermcu_t *p, long fd __attribute__((unused))) -{ - pprz_msg_send_IMCU_TELEMETRY(&(intermcu.transport.trans_tx), intermcu.device, - INTERMCU_AP, p->buf_idx, p->buf); - p->buf_idx = 0; -} diff --git a/sw/airborne/modules/telemetry/telemetry_intermcu_ap.h b/sw/airborne/modules/telemetry/telemetry_intermcu_ap.h deleted file mode 100644 index 3680ed6a86..0000000000 --- a/sw/airborne/modules/telemetry/telemetry_intermcu_ap.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright (C) 2016 Freek van Tienen - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** @file modules/telemetry/telemetry_intermcu_ap.h - * @brief Telemetry through InterMCU - */ - -#ifndef TELEMETRY_INTERMCU_AP_H -#define TELEMETRY_INTERMCU_AP_H - -#include "std.h" -#include "pprzlink/short_transport.h" - -/* Default maximum telemetry message size */ -#ifndef TELEMERTY_INTERMCU_MSG_SIZE -#define TELEMERTY_INTERMCU_MSG_SIZE 128 -#endif - -/* Structure for handling telemetry over InterMCU */ -struct telemetry_intermcu_t { - struct link_device dev; ///< Device structure for communication - struct short_transport trans; ///< Transport without any extra encoding - uint8_t buf[TELEMERTY_INTERMCU_MSG_SIZE]; ///< Buffer for the messages - uint8_t buf_idx; ///< Index of the buffer -}; - -/* Telemetry InterMCU throughput */ -extern struct telemetry_intermcu_t telemetry_intermcu; - -#endif /* TELEMETRY_INTERMCU_AP_H */ diff --git a/sw/airborne/modules/telemetry/telemetry_intermcu_fbw.c b/sw/airborne/modules/telemetry/telemetry_intermcu_fbw.c deleted file mode 100644 index 290d860ded..0000000000 --- a/sw/airborne/modules/telemetry/telemetry_intermcu_fbw.c +++ /dev/null @@ -1,120 +0,0 @@ -/* - * Copyright (C) 2016 Freek van Tienen - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** @file modules/telemetry/telemetry_intermcu_fbw.c - * @brief Telemetry through InterMCU - */ - -#include "telemetry_intermcu.h" -#include "modules/intermcu/intermcu.h" -#include "pprzlink/pprz_transport.h" -#include "pprzlink/intermcu_msg.h" -#include "modules/datalink/telemetry.h" -#include "firmwares/rotorcraft/main_fbw.h" - -#define MSG_SIZE 256 - -extern fbw_mode_enum fbw_mode; - -/* Structure for handling telemetry over InterMCU */ -struct telemetry_intermcu_t { - struct link_device *dev; ///< Device structure for communication - struct pprz_transport trans; ///< Transport without any extra encoding - - uint8_t rx_buffer[MSG_SIZE]; ///< Received bytes from datalink - bool msg_received; ///< Whenever a datalink message is received -}; - -/* Telemetry InterMCU throughput */ -static struct telemetry_intermcu_t telemetry_intermcu; - -/* Statically defined functions */ -static void telemetry_intermcu_repack(struct transport_tx *trans, struct link_device *dev, uint8_t ac_id, uint8_t *msg, uint8_t size); - -/* InterMCU initialization */ -void telemetry_intermcu_init(void) -{ - // Initialize transport structure - pprz_transport_init(&telemetry_intermcu.trans); - - // Set the link device - telemetry_intermcu.dev = &(TELEMETRY_INTERMCU_DEV.device); -} - -/* InterMCU periodic handling of telemetry */ -void telemetry_intermcu_periodic(void) -{ - -} - -/* InterMCU event handling of telemetry */ -void telemetry_intermcu_event(void) -{ - pprz_check_and_parse(&(TELEMETRY_INTERMCU_DEV).device, &telemetry_intermcu.trans, telemetry_intermcu.rx_buffer, &telemetry_intermcu.msg_received); - if(telemetry_intermcu.msg_received) { - /* Switch on MSG ID */ - switch(pprzlink_get_msg_id(telemetry_intermcu.rx_buffer)) { - case DL_EMERGENCY_CMD: - if(DL_EMERGENCY_CMD_ac_id(telemetry_intermcu.rx_buffer) == AC_ID - && DL_EMERGENCY_CMD_cmd(telemetry_intermcu.rx_buffer) == 0) { - fbw_mode = FBW_MODE_FAILSAFE; - } - break; - - default: - break; - } - - /* Forward to AP */ - pprz_msg_send_IMCU_DATALINK(&(intermcu.transport.trans_tx), intermcu.device, - INTERMCU_FBW, telemetry_intermcu.trans.trans_rx.payload_len, telemetry_intermcu.rx_buffer); - - telemetry_intermcu.msg_received = false; - } -} - -void telemetry_intermcu_on_msg(uint8_t* msg, uint8_t size) -{ - telemetry_intermcu_repack(&(telemetry_intermcu.trans.trans_tx), telemetry_intermcu.dev, AC_ID, msg, size); -} - -static void telemetry_intermcu_repack(struct transport_tx *trans, struct link_device *dev, uint8_t ac_id,uint8_t *msg, uint8_t size) -{ -#if PPRZLINK_DEFAULT_VER == 2 - struct pprzlink_msg pmsg; - pmsg.trans = trans; - pmsg.dev = dev; - pmsg.sender_id = ac_id; - pmsg.receiver_id = 0; - pmsg.component_id = 0; - - trans->count_bytes(&pmsg, size); - trans->start_message(&pmsg, _FD, size); - trans->put_bytes(&pmsg, _FD, DL_TYPE_UINT8, DL_FORMAT_ARRAY, (void *) msg, size); - trans->end_message(&pmsg, _FD); -#else - trans->count_bytes(trans->impl, dev, trans->size_of(trans->impl, size)); - trans->start_message(trans->impl, dev, 0, size); - trans->put_bytes(trans->impl, dev, 0, DL_TYPE_UINT8, DL_FORMAT_ARRAY, (void *) msg, size); - trans->end_message(trans->impl, dev, 0); -#endif -} diff --git a/sw/airborne/modules/uav_recovery/uav_recovery.c b/sw/airborne/modules/uav_recovery/uav_recovery.c index 17b7ade8a9..0fc9bd4bcc 100644 --- a/sw/airborne/modules/uav_recovery/uav_recovery.c +++ b/sw/airborne/modules/uav_recovery/uav_recovery.c @@ -31,9 +31,9 @@ #include "modules/nav/common_nav.h" #include "generated/flight_plan.h" #include "generated/airframe.h" -#include "modules/intermcu/inter_mcu.h" #include "modules/datalink/datalink.h" #include "modules/multi/traffic_info.h" +#include "modules/core/commands.h" #include "uav_recovery.h" #if defined(USE_PARACHUTE) && USE_PARACHUTE == 1 @@ -100,7 +100,7 @@ void uav_recovery_init(void) wind_measurements_valid = true; wind_info_valid = true; #if defined(PARACHUTE_OUTPUT_COMMAND) - ap_state->commands[PARACHUTE_OUTPUT_COMMAND] = MIN_PPRZ; + command_set(PARACHUTE_OUTPUT_COMMAND, MIN_PPRZ); #endif #if PERIODIC_TELEMETRY @@ -117,9 +117,9 @@ void uav_recovery_periodic(void) #if defined(PARACHUTE_OUTPUT_COMMAND) if (autopilot.mode != AP_MODE_MANUAL) { if (deploy_parachute_var == 1) { - ap_state->commands[PARACHUTE_OUTPUT_COMMAND] = MAX_PPRZ; + command_set(PARACHUTE_OUTPUT_COMMAND, MAX_PPRZ); - } else { ap_state->commands[PARACHUTE_OUTPUT_COMMAND] = MIN_PPRZ; } + } else { command_set(PARACHUTE_OUTPUT_COMMAND, MIN_PPRZ); } } #else #warning PARACHUTE COMMAND NOT FOUND diff --git a/sw/airborne/test/modules/test_radio_control.c b/sw/airborne/test/modules/test_radio_control.c index 14b85ea81f..f8eee2a7e0 100644 --- a/sw/airborne/test/modules/test_radio_control.c +++ b/sw/airborne/test/modules/test_radio_control.c @@ -83,13 +83,12 @@ static inline void main_periodic_task(void) {uint8_t blaa = 0; DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice, &blaa, 8, ppm_pulses);}); #endif - LED_PERIODIC(); } static inline void main_event_task(void) { mcu_event(); - RadioControlEvent(main_on_radio_control_frame); + //RadioControlEvent(main_on_radio_control_frame); FIXME } static void main_on_radio_control_frame(void) diff --git a/sw/lib/ocaml/module.ml b/sw/lib/ocaml/module.ml index 9d01cd035b..3ca10d5f70 100644 --- a/sw/lib/ocaml/module.ml +++ b/sw/lib/ocaml/module.ml @@ -201,7 +201,14 @@ let make_event = fun f cond -> } -type datalink = { message: string; func: string } +type datalink = { message: string; func: string; dl_class: string option; cond: string option } + +let make_datalink = fun f m cl cond -> + { message = m; + func = f; + dl_class = cl; + cond = cond + } let fprint_datalink = fun ch d -> Printf.fprintf ch "(msg_id == DL_%s) { %s; }\n" d.message d.func @@ -312,8 +319,10 @@ let rec parse_xml m = function { m with events = make_event f c :: m.events } | Xml.Element ("datalink", _, []) as xml -> let message = Xml.attrib xml "message" - and func = Xml.attrib xml "fun" in - { m with datalinks = { message; func } :: m.datalinks } + and func = Xml.attrib xml "fun" + and dl_class = ExtXml.attrib_opt xml "class" + and c = ExtXml.attrib_opt xml "cond" in + { m with datalinks = make_datalink func message dl_class c :: m.datalinks } | Xml.Element ("makefile", _, _) as xml -> { m with makefiles = parse_makefile empty_makefile xml :: m.makefiles } | _ -> failwith "Module.parse_xml: unreachable" diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index db66ef7bd1..eb30ed89f8 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -22,25 +22,14 @@ #include "nps_autopilot.h" -#ifdef FBW -#include "firmwares/fixedwing/main_fbw.h" -#define Fbw(f) f ## _fbw() -#else -#define Fbw(f) -#endif - -#ifdef AP -#include "firmwares/fixedwing/main_ap.h" -#define Ap(f) f ## _ap() -#else -#define Ap(f) -#endif +#include "main_ap.h" #include "nps_sensors.h" #include "nps_radio_control.h" #include "nps_electrical.h" #include "nps_fdm.h" +#include "generated/modules.h" #include "modules/radio_control/radio_control.h" #include "modules/imu/imu.h" #include "mcu_periph/sys_time.h" @@ -73,9 +62,9 @@ bool nps_bypass_ins; #endif -#if !defined (FBW) || !defined (AP) -#error NPS does not currently support dual processor simulation for FBW and AP on fixedwing! -#endif +//#if !defined (FBW) || !defined (AP) +//#error NPS does not currently support dual processor simulation for FBW and AP on fixedwing! +//#endif void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev) { @@ -90,8 +79,8 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha nps_bypass_ahrs = NPS_BYPASS_AHRS; nps_bypass_ins = NPS_BYPASS_INS; - Fbw(init); - Ap(init); + modules_mcu_init(); + main_ap_init(); } @@ -111,28 +100,25 @@ void nps_autopilot_run_step(double time) #if RADIO_CONTROL && !RADIO_CONTROL_TYPE_DATALINK if (nps_radio_control_available(time)) { radio_control_feed(); - Fbw(event_task); + main_ap_event(); } #endif if (nps_sensors_gyro_available()) { imu_feed_gyro_accel(); - Fbw(event_task); - Ap(event_task); + main_ap_event(); } if (nps_sensors_mag_available()) { imu_feed_mag(); - Fbw(event_task); - Ap(event_task); + main_ap_event(); } if (nps_sensors_baro_available()) { uint32_t now_ts = get_sys_time_usec(); float pressure = (float) sensors.baro.value; AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, now_ts, pressure); - Fbw(event_task); - Ap(event_task); + main_ap_event(); } if (nps_sensors_temperature_available()) { @@ -142,15 +128,13 @@ void nps_autopilot_run_step(double time) #if USE_AIRSPEED || USE_NPS_AIRSPEED if (nps_sensors_airspeed_available()) { AbiSendMsgAIRSPEED(AIRSPEED_NPS_ID, (float)sensors.airspeed.value); - Fbw(event_task); - Ap(event_task); + main_ap_event(); } #endif if (nps_sensors_gps_available()) { gps_feed_value(); - Fbw(event_task); - Ap(event_task); + main_ap_event(); } #if USE_SONAR @@ -164,8 +148,7 @@ void nps_autopilot_run_step(double time) DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist); #endif - Fbw(event_task); - Ap(event_task); + main_ap_event(); } #endif @@ -173,8 +156,7 @@ void nps_autopilot_run_step(double time) #if USE_NPS_AOA && !NPS_SYNC_INCIDENCE if (nps_sensors_aoa_available()) { AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 1, (float)sensors.aoa.value, 0.f); - Fbw(event_task); - Ap(event_task); + main_ap_event(); } #endif @@ -182,8 +164,7 @@ void nps_autopilot_run_step(double time) #if USE_NPS_SIDESLIP && !NPS_SYNC_INCIDENCE if (nps_sensors_sideslip_available()) { AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 2, 0.f, (float)sensors.sideslip.value); - Fbw(event_task); - Ap(event_task); + main_ap_event(); } #endif @@ -199,8 +180,7 @@ void nps_autopilot_run_step(double time) if (flag == 3) { // both sensors are updated AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 3, (float)sensors.aoa.value, (float)sensors.sideslip.value); - Fbw(event_task); - Ap(event_task); + main_ap_event(); flag = 0; } #endif @@ -215,8 +195,7 @@ void nps_autopilot_run_step(double time) sim_overwrite_ins(); } - Fbw(handle_periodic_tasks); - Ap(handle_periodic_tasks); + main_ap_periodic(); /* scale final motor commands to 0-1 for feeding the fdm */ #ifdef NPS_ACTUATOR_NAMES diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index 5d74aeb4a5..e74a31a650 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -21,7 +21,7 @@ #include "nps_autopilot.h" -#include "firmwares/rotorcraft/main.h" +#include "main_ap.h" #include "nps_sensors.h" #include "nps_radio_control.h" #include "nps_electrical.h" @@ -78,8 +78,8 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha nps_bypass_ahrs = NPS_BYPASS_AHRS; nps_bypass_ins = NPS_BYPASS_INS; - main_init(); - + modules_mcu_init(); + main_ap_init(); } void nps_autopilot_run_systime_step(void) @@ -98,35 +98,37 @@ void nps_autopilot_run_step(double time) #if RADIO_CONTROL && !RADIO_CONTROL_TYPE_DATALINK if (nps_radio_control_available(time)) { radio_control_feed(); - main_event(); + main_ap_event(); } #endif if (nps_sensors_gyro_available()) { imu_feed_gyro_accel(); - main_event(); + main_ap_event(); } if (nps_sensors_mag_available()) { imu_feed_mag(); - main_event(); + main_ap_event(); } if (nps_sensors_baro_available()) { uint32_t now_ts = get_sys_time_usec(); float pressure = (float) sensors.baro.value; AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, now_ts, pressure); - main_event(); + main_ap_event(); } if (nps_sensors_temperature_available()) { AbiSendMsgTEMPERATURE(BARO_SIM_SENDER_ID, (float)sensors.temp.value); + main_ap_event(); } #if USE_AIRSPEED if (nps_sensors_airspeed_available()) { stateSetAirspeed_f((float)sensors.airspeed.value); AbiSendMsgAIRSPEED(AIRSPEED_NPS_ID, (float)sensors.airspeed.value); + main_ap_event(); } #endif @@ -143,14 +145,14 @@ void nps_autopilot_run_step(double time) DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist); #endif - main_event(); + main_ap_event(); } #endif #if USE_GPS if (nps_sensors_gps_available()) { gps_feed_value(); - main_event(); + main_ap_event(); } #endif @@ -162,7 +164,7 @@ void nps_autopilot_run_step(double time) sim_overwrite_ins(); } - handle_periodic_tasks(); + main_ap_periodic(); /* scale final motor commands to 0-1 for feeding the fdm */ for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) { diff --git a/sw/tools/generators/gen_modules.ml b/sw/tools/generators/gen_modules.ml index 2125884835..7261b979d3 100644 --- a/sw/tools/generators/gen_modules.ml +++ b/sw/tools/generators/gen_modules.ml @@ -312,21 +312,65 @@ let print_event_functions = fun out modules -> let print_datalink_functions = fun out modules -> lprintf out "\n#include \"pprzlink/messages.h\"\n"; + lprintf out "\n#include \"pprzlink/dl_protocol.h\"\n"; + lprintf out "\n#include \"pprzlink/intermcu_msg.h\"\n"; lprintf out "#include \"generated/airframe.h\"\n"; - lprintf out "static inline void modules_parse_datalink(uint8_t msg_id __attribute__ ((unused)), + lprintf out "static inline void modules_parse_datalink(uint8_t msg_id __attribute__((unused)), + uint8_t class_id __attribute__((unused)), struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf __attribute__((unused))) {\n"; right (); - let else_ = ref "" in + let msgs = Hashtbl.create 3 in List.iter (fun m -> List.iter (fun d -> - lprintf out "%sif (msg_id == DL_%s) { %s; }\n" !else_ d.Module.message d.Module.func; - else_ := "else " + if not (Hashtbl.mem msgs d.Module.dl_class) then + Hashtbl.add msgs d.Module.dl_class (Hashtbl.create 15); + let c = Hashtbl.find msgs d.Module.dl_class in + let new_cb = (d.Module.func, d.Module.cond) in + if Hashtbl.mem c d.Module.message then + Hashtbl.replace c d.Module.message (new_cb :: Hashtbl.find c d.Module.message) + else + Hashtbl.add c d.Module.message [new_cb] ) m.Module.datalinks ) modules; + + lprintf out "switch (class_id) {\n"; + right (); + Hashtbl.iter (fun class_name msg_tbl -> + let class_name = match class_name with None -> "datalink" | Some x -> x in + lprintf out "case DL_%s_CLASS_ID: {\n" class_name; + right (); + lprintf out "switch (msg_id) {\n"; + right (); + Hashtbl.iter (fun msg_name _ -> + if compare msg_name "*" != 0 then begin (* skip wildcard *) + lprintf out "case DL_%s: {\n" msg_name; + right (); + List.iter (fun (cb, cond) -> + lprintf_with_cond out cb cond; + ) (Hashtbl.find msg_tbl msg_name); + lprintf out "break;\n"; + left (); + lprintf out "}\n" (* close msg case *) + end + ) msg_tbl; + lprintf out "default: break;\n"; + left (); + lprintf out "}\n"; (* close msg switch *) + Hashtbl.iter (fun msg_name _ -> + if compare msg_name "*" = 0 then (* callbacks for wildcard *) + List.iter (fun (cb, cond) -> lprintf_with_cond out cb cond) (Hashtbl.find msg_tbl msg_name) + ) msg_tbl; + left (); + lprintf out " break;\n"; + lprintf out "}\n"; (* close class case *) + ) msgs; + lprintf out "default: break;\n"; left (); - lprintf out "}\n" + lprintf out "}\n"; (* close class switch *) + left (); + lprintf out "}\n" (* close function *) let parse_modules out modules = print_headers out modules; diff --git a/tests/modules/generated/airframe.h b/tests/modules/generated/airframe.h index 9a18c7f74c..ec58cd66fc 100644 --- a/tests/modules/generated/airframe.h +++ b/tests/modules/generated/airframe.h @@ -17,6 +17,11 @@ #define COMMAND_YAW 2 #define COMMAND_THRUST 3 +#define SetCommandsFromRC(_commands_array, _rc_array) { } +#define AllActuatorsInit() { } +#define AllActuatorsCommit() { } +#define SetActuatorsFromCommands(values, AP_MODE) { } + #define KILL_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME) #define DEFAULT_CIRCLE_RADIUS 80. #define NOMINAL_AIRSPEED 12. diff --git a/tests/modules/generated/modules.h b/tests/modules/generated/modules.h index ab0e2a4c5c..70e0efdb24 100644 --- a/tests/modules/generated/modules.h +++ b/tests/modules/generated/modules.h @@ -6,14 +6,108 @@ #define MODULES_START 2 #define MODULES_STOP 3 +#include "std.h" +#include "core/settings.h" +#include "radio_control/radio_control.h" +#include "./mcu.h" +#include "mcu_periph/gpio.h" +#include "math/pprz_algebra_int.h" +#include "math/pprz_algebra_float.h" +#include "math/pprz_algebra_double.h" +#include "math/pprz_geodetic_int.h" +#include "math/pprz_geodetic_float.h" +#include "math/pprz_geodetic_double.h" +#include "math/pprz_trig_int.h" +#include "math/pprz_orientation_conversion.h" +#include "math/pprz_stat.h" +#include "imu/imu.h" +#include "mcu_periph/i2c.h" +#include "gps/gps.h" +#include "air_data/air_data.h" +#include "ahrs/ahrs.h" +#include "actuators/actuators.h" +#include "mcu_periph/adc.h" +#include "energy/electrical.h" +#include "mcu_periph/spi.h" +#include "./state.h" +#include "mcu_periph/uart.h" +#include "core/commands.h" + // dummy variables extern int nav_catapult_nav_catapult_highrate_module_status; +static inline void modules_mcu_init(void) {} +static inline void modules_core_init(void) {} +static inline void modules_sensors_init(void) {} +static inline void modules_estimation_init(void) {} +static inline void modules_radio_control_init(void) {} +static inline void modules_control_init(void) {} +static inline void modules_actuators_init(void) {} +static inline void modules_datalink_init(void) {} +static inline void modules_default_init(void) {} + +static inline void modules_init(void) { + modules_mcu_init(); + modules_core_init(); + modules_sensors_init(); + modules_estimation_init(); + modules_radio_control_init(); + modules_control_init(); + modules_actuators_init(); + modules_datalink_init(); + modules_default_init(); +} + +static inline void modules_mcu_periodic_task(void) {} +static inline void modules_core_periodic_task(void) {} +static inline void modules_sensors_periodic_task(void) {} +static inline void modules_estimation_periodic_task(void) {} +static inline void modules_radio_control_periodic_task(void) {} +static inline void modules_control_periodic_task(void) {} +static inline void modules_actuators_periodic_task(void) {} +static inline void modules_datalink_periodic_task(void) {} +static inline void modules_default_periodic_task(void) {} + +static inline void modules_periodic_task(void) { + modules_mcu_periodic_task(); + modules_core_periodic_task(); + modules_sensors_periodic_task(); + modules_estimation_periodic_task(); + modules_radio_control_periodic_task(); + modules_control_periodic_task(); + modules_actuators_periodic_task(); + modules_datalink_periodic_task(); + modules_default_periodic_task(); +} + +static inline void modules_mcu_event_task(void) {} +static inline void modules_core_event_task(void) {} +static inline void modules_sensors_event_task(void) {} +static inline void modules_estimation_event_task(void) {} +static inline void modules_radio_control_event_task(void) {} +static inline void modules_control_event_task(void) {} +static inline void modules_actuators_event_task(void) {} +static inline void modules_datalink_event_task(void) {} +static inline void modules_default_event_task(void) {} + +static inline void modules_event_task(void) { + modules_mcu_event_task(); + modules_core_event_task(); + modules_sensors_event_task(); + modules_estimation_event_task(); + modules_radio_control_event_task(); + modules_control_event_task(); + modules_actuators_event_task(); + modules_datalink_event_task(); + modules_default_event_task(); +} + #ifdef MODULES_DATALINK_C #include "pprzlink/messages.h" #include "generated/airframe.h" static inline void modules_parse_datalink(uint8_t msg_id __attribute__ ((unused)), + uint8_t class_id __attribute__((unused)), struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf __attribute__((unused))) { diff --git a/tests/modules/generated/periodic_telemetry.h b/tests/modules/generated/periodic_telemetry.h index 65a8f17292..965d2cb321 100644 --- a/tests/modules/generated/periodic_telemetry.h +++ b/tests/modules/generated/periodic_telemetry.h @@ -9,6 +9,24 @@ #define TELEMETRY_PPRZ_NB_MSG 1 #define TELEMETRY_PPRZ_CBS { 0 } +static inline void periodic_telemetry_send_Ap(struct periodic_telemetry *telemetry, struct transport_tx *trans, struct link_device *dev) { + (void)telemetry; + (void)trans; + (void)dev; +} + +static inline void periodic_telemetry_send_Main(struct periodic_telemetry *telemetry, struct transport_tx *trans, struct link_device *dev) { + (void)telemetry; + (void)trans; + (void)dev; +} + +static inline void periodic_telemetry_send_Fbw(struct periodic_telemetry *telemetry, struct transport_tx *trans, struct link_device *dev) { + (void)telemetry; + (void)trans; + (void)dev; +} + static inline void periodic_telemetry_send_InterMCU(struct periodic_telemetry *telemetry, struct transport_tx *trans, struct link_device *dev) { (void)telemetry; (void)trans;