From 21e88f64b4c80887cd079a05f46666ce117039c1 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Wed, 4 Jan 2023 17:14:00 +0100 Subject: [PATCH] Add new fixed wing rate control module (fw_rate_control) * Fixedwing rate control into a separate module * Start fw_rate_control for vtol * Move over airspeed related parameters to fw_rate_control Co-authored-by: Silvan Fuhrer --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 1 + ROMFS/px4fmu_common/init.d/rc.vtol_apps | 1 + boards/airmind/mindpx-v2/default.px4board | 1 + boards/ark/fmu-v6x/default.px4board | 1 + boards/atl/mantis-edu/default.px4board | 2 - boards/av/x-v1/default.px4board | 1 + boards/beaglebone/blue/default.px4board | 1 + boards/cuav/nora/default.px4board | 1 + boards/cuav/x7pro/default.px4board | 1 + boards/cubepilot/cubeorange/default.px4board | 1 + boards/cubepilot/cubeyellow/default.px4board | 1 + boards/emlid/navio2/default.px4board | 1 + boards/holybro/durandal-v1/default.px4board | 1 + boards/holybro/kakuteh7/default.px4board | 1 + boards/holybro/pix32v5/default.px4board | 1 + boards/matek/h743-mini/default.px4board | 1 + boards/matek/h743-slim/default.px4board | 1 + boards/matek/h743/default.px4board | 1 + boards/modalai/fc-v1/default.px4board | 1 + boards/modalai/fc-v2/default.px4board | 1 + boards/mro/ctrl-zero-classic/default.px4board | 1 + boards/mro/ctrl-zero-f7-oem/default.px4board | 1 + boards/mro/ctrl-zero-f7/default.px4board | 1 + boards/mro/ctrl-zero-h7-oem/default.px4board | 1 + boards/mro/ctrl-zero-h7/default.px4board | 1 + boards/mro/pixracerpro/default.px4board | 1 + boards/mro/x21-777/default.px4board | 1 + boards/mro/x21/default.px4board | 1 + boards/nxp/fmuk66-e/default.px4board | 1 + boards/nxp/fmuk66-v3/default.px4board | 1 + boards/px4/fmu-v2/fixedwing.px4board | 1 + boards/px4/fmu-v3/default.px4board | 1 + boards/px4/fmu-v4/default.px4board | 1 + boards/px4/fmu-v4pro/default.px4board | 1 + boards/px4/fmu-v5/default.px4board | 1 + boards/px4/fmu-v5/uavcanv0periph.px4board | 2 + boards/px4/fmu-v5x/default.px4board | 1 + boards/px4/fmu-v6c/default.px4board | 1 + boards/px4/fmu-v6u/default.px4board | 1 + boards/px4/fmu-v6x/default.px4board | 1 + boards/px4/raspberrypi/default.px4board | 1 + boards/px4/sitl/default.px4board | 1 + boards/scumaker/pilotpi/default.px4board | 1 + .../smartap-airlink/default.px4board | 1 + src/modules/fw_att_control/CMakeLists.txt | 2 - .../FixedwingAttitudeControl.cpp | 534 ++------------ .../FixedwingAttitudeControl.hpp | 139 +--- src/modules/fw_att_control/ecl_controller.cpp | 15 - src/modules/fw_att_control/ecl_controller.h | 8 +- .../fw_att_control/ecl_pitch_controller.cpp | 3 +- .../fw_att_control/ecl_yaw_controller.cpp | 6 +- .../fw_att_control/fw_att_control_params.c | 567 --------------- .../FixedwingPositionControl.cpp | 5 - .../fw_pos_control_l1_params.c | 69 -- src/modules/fw_rate_control/CMakeLists.txt | 44 ++ .../fw_rate_control/FixedwingRateControl.cpp | 624 ++++++++++++++++ .../fw_rate_control/FixedwingRateControl.hpp | 245 +++++++ src/modules/fw_rate_control/Kconfig | 12 + .../fw_rate_control/fw_rate_control_params.c | 677 ++++++++++++++++++ 59 files changed, 1736 insertions(+), 1260 deletions(-) create mode 100644 src/modules/fw_rate_control/CMakeLists.txt create mode 100644 src/modules/fw_rate_control/FixedwingRateControl.cpp create mode 100644 src/modules/fw_rate_control/FixedwingRateControl.hpp create mode 100644 src/modules/fw_rate_control/Kconfig create mode 100644 src/modules/fw_rate_control/fw_rate_control_params.c diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 2e2550884a..426a9b83d1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -18,6 +18,7 @@ control_allocator start # # Start attitude controller. # +fw_rate_control start fw_att_control start fw_pos_control_l1 start airspeed_selector start diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 0bb53168d1..56ba89f99d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -35,6 +35,7 @@ then mc_autotune_attitude_control start fi +fw_rate_control start vtol fw_att_control start vtol fw_pos_control_l1 start vtol fw_autotune_attitude_control start vtol diff --git a/boards/airmind/mindpx-v2/default.px4board b/boards/airmind/mindpx-v2/default.px4board index 5083e338cc..4d6c0e9eac 100644 --- a/boards/airmind/mindpx-v2/default.px4board +++ b/boards/airmind/mindpx-v2/default.px4board @@ -46,6 +46,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/ark/fmu-v6x/default.px4board b/boards/ark/fmu-v6x/default.px4board index f43f85707a..3b168411d0 100644 --- a/boards/ark/fmu-v6x/default.px4board +++ b/boards/ark/fmu-v6x/default.px4board @@ -41,6 +41,7 @@ CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y diff --git a/boards/atl/mantis-edu/default.px4board b/boards/atl/mantis-edu/default.px4board index 4434e35126..b10c73001f 100644 --- a/boards/atl/mantis-edu/default.px4board +++ b/boards/atl/mantis-edu/default.px4board @@ -14,8 +14,6 @@ CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y -CONFIG_MODULES_FW_ATT_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/av/x-v1/default.px4board b/boards/av/x-v1/default.px4board index 69afaab664..68b9afc95a 100644 --- a/boards/av/x-v1/default.px4board +++ b/boards/av/x-v1/default.px4board @@ -43,6 +43,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/beaglebone/blue/default.px4board b/boards/beaglebone/blue/default.px4board index 7cef007869..9a8af3d37d 100644 --- a/boards/beaglebone/blue/default.px4board +++ b/boards/beaglebone/blue/default.px4board @@ -30,6 +30,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/cuav/nora/default.px4board b/boards/cuav/nora/default.px4board index 1c21a35230..fb51081280 100644 --- a/boards/cuav/nora/default.px4board +++ b/boards/cuav/nora/default.px4board @@ -50,6 +50,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/cuav/x7pro/default.px4board b/boards/cuav/x7pro/default.px4board index d4ae8ad96a..aadfd907b8 100644 --- a/boards/cuav/x7pro/default.px4board +++ b/boards/cuav/x7pro/default.px4board @@ -51,6 +51,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index f04db4d636..a28818a424 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -44,6 +44,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/cubepilot/cubeyellow/default.px4board b/boards/cubepilot/cubeyellow/default.px4board index add9907ced..3d1158840b 100644 --- a/boards/cubepilot/cubeyellow/default.px4board +++ b/boards/cubepilot/cubeyellow/default.px4board @@ -46,6 +46,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/emlid/navio2/default.px4board b/boards/emlid/navio2/default.px4board index d1bab3fd5c..09ac9a68e7 100644 --- a/boards/emlid/navio2/default.px4board +++ b/boards/emlid/navio2/default.px4board @@ -32,6 +32,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/holybro/durandal-v1/default.px4board b/boards/holybro/durandal-v1/default.px4board index 1823d71781..ba7a479cc4 100644 --- a/boards/holybro/durandal-v1/default.px4board +++ b/boards/holybro/durandal-v1/default.px4board @@ -44,6 +44,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 29d7f7917d..7908a8c254 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -37,6 +37,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/holybro/pix32v5/default.px4board b/boards/holybro/pix32v5/default.px4board index 448430dcf9..0bab679b38 100644 --- a/boards/holybro/pix32v5/default.px4board +++ b/boards/holybro/pix32v5/default.px4board @@ -50,6 +50,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/matek/h743-mini/default.px4board b/boards/matek/h743-mini/default.px4board index d04fcfe761..32cccaa01e 100644 --- a/boards/matek/h743-mini/default.px4board +++ b/boards/matek/h743-mini/default.px4board @@ -29,6 +29,7 @@ CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/matek/h743-slim/default.px4board b/boards/matek/h743-slim/default.px4board index 23c459c9c9..5b88b2aef9 100644 --- a/boards/matek/h743-slim/default.px4board +++ b/boards/matek/h743-slim/default.px4board @@ -30,6 +30,7 @@ CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/matek/h743/default.px4board b/boards/matek/h743/default.px4board index ee9ee7d89e..3ccb9cb0aa 100644 --- a/boards/matek/h743/default.px4board +++ b/boards/matek/h743/default.px4board @@ -31,6 +31,7 @@ CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index 5353dd0ceb..8e824d712d 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -48,6 +48,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board index 83ac665636..2cb899a63f 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -45,6 +45,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/mro/ctrl-zero-classic/default.px4board b/boards/mro/ctrl-zero-classic/default.px4board index e4be8f43ff..ac50538bd4 100644 --- a/boards/mro/ctrl-zero-classic/default.px4board +++ b/boards/mro/ctrl-zero-classic/default.px4board @@ -43,6 +43,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/mro/ctrl-zero-f7-oem/default.px4board b/boards/mro/ctrl-zero-f7-oem/default.px4board index ac3f9457a1..60af3542c9 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.px4board +++ b/boards/mro/ctrl-zero-f7-oem/default.px4board @@ -44,6 +44,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/mro/ctrl-zero-f7/default.px4board b/boards/mro/ctrl-zero-f7/default.px4board index 971c7b0914..6a073fcbf3 100644 --- a/boards/mro/ctrl-zero-f7/default.px4board +++ b/boards/mro/ctrl-zero-f7/default.px4board @@ -44,6 +44,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/mro/ctrl-zero-h7-oem/default.px4board b/boards/mro/ctrl-zero-h7-oem/default.px4board index e4be8f43ff..ac50538bd4 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.px4board +++ b/boards/mro/ctrl-zero-h7-oem/default.px4board @@ -43,6 +43,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/mro/ctrl-zero-h7/default.px4board b/boards/mro/ctrl-zero-h7/default.px4board index 35e4a5110e..51c3ea0ada 100644 --- a/boards/mro/ctrl-zero-h7/default.px4board +++ b/boards/mro/ctrl-zero-h7/default.px4board @@ -44,6 +44,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/mro/pixracerpro/default.px4board b/boards/mro/pixracerpro/default.px4board index 7089208dc7..cfe647afc3 100644 --- a/boards/mro/pixracerpro/default.px4board +++ b/boards/mro/pixracerpro/default.px4board @@ -43,6 +43,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/mro/x21-777/default.px4board b/boards/mro/x21-777/default.px4board index 7ec97627aa..b395fc1917 100644 --- a/boards/mro/x21-777/default.px4board +++ b/boards/mro/x21-777/default.px4board @@ -45,6 +45,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/mro/x21/default.px4board b/boards/mro/x21/default.px4board index 7461a68627..fdd26fe9ca 100644 --- a/boards/mro/x21/default.px4board +++ b/boards/mro/x21/default.px4board @@ -46,6 +46,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/nxp/fmuk66-e/default.px4board b/boards/nxp/fmuk66-e/default.px4board index dfab3337cd..8a34a91436 100644 --- a/boards/nxp/fmuk66-e/default.px4board +++ b/boards/nxp/fmuk66-e/default.px4board @@ -46,6 +46,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/nxp/fmuk66-v3/default.px4board b/boards/nxp/fmuk66-v3/default.px4board index 080a841a7e..162f3cf00a 100644 --- a/boards/nxp/fmuk66-v3/default.px4board +++ b/boards/nxp/fmuk66-v3/default.px4board @@ -48,6 +48,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/fmu-v2/fixedwing.px4board b/boards/px4/fmu-v2/fixedwing.px4board index 89fa7703c9..8fcae53b89 100644 --- a/boards/px4/fmu-v2/fixedwing.px4board +++ b/boards/px4/fmu-v2/fixedwing.px4board @@ -9,4 +9,5 @@ CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y diff --git a/boards/px4/fmu-v3/default.px4board b/boards/px4/fmu-v3/default.px4board index d504414330..5398c18cd4 100644 --- a/boards/px4/fmu-v3/default.px4board +++ b/boards/px4/fmu-v3/default.px4board @@ -49,6 +49,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/fmu-v4/default.px4board b/boards/px4/fmu-v4/default.px4board index 4e3ecf30e3..a1ea6a2140 100644 --- a/boards/px4/fmu-v4/default.px4board +++ b/boards/px4/fmu-v4/default.px4board @@ -50,6 +50,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/fmu-v4pro/default.px4board b/boards/px4/fmu-v4pro/default.px4board index 62fa4987aa..ea8dba8915 100644 --- a/boards/px4/fmu-v4pro/default.px4board +++ b/boards/px4/fmu-v4pro/default.px4board @@ -47,6 +47,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index 46a32edfec..d4441ec4d7 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -55,6 +55,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/fmu-v5/uavcanv0periph.px4board b/boards/px4/fmu-v5/uavcanv0periph.px4board index 2b77db79bf..5cc3e00d0d 100644 --- a/boards/px4/fmu-v5/uavcanv0periph.px4board +++ b/boards/px4/fmu-v5/uavcanv0periph.px4board @@ -19,7 +19,9 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n CONFIG_MODULES_CAMERA_FEEDBACK=n CONFIG_MODULES_ESC_BATTERY=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_GYRO_FFT=n CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_MICRODDS_CLIENT=n CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=n diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index bb7017ee81..ed606e932d 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -55,6 +55,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index d62f84406d..e85cf91bdb 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -40,6 +40,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/fmu-v6u/default.px4board b/boards/px4/fmu-v6u/default.px4board index 6fadd08b42..e26336aa1b 100644 --- a/boards/px4/fmu-v6u/default.px4board +++ b/boards/px4/fmu-v6u/default.px4board @@ -47,6 +47,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index ee85f7ccc2..375d7bd02f 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -49,6 +49,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/raspberrypi/default.px4board b/boards/px4/raspberrypi/default.px4board index 74aca7c7d8..cb52269076 100644 --- a/boards/px4/raspberrypi/default.px4board +++ b/boards/px4/raspberrypi/default.px4board @@ -29,6 +29,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index b9b80c08d1..c41b834caf 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -16,6 +16,7 @@ CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/scumaker/pilotpi/default.px4board b/boards/scumaker/pilotpi/default.px4board index ab673e0186..c3656660a1 100644 --- a/boards/scumaker/pilotpi/default.px4board +++ b/boards/scumaker/pilotpi/default.px4board @@ -29,6 +29,7 @@ CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/sky-drones/smartap-airlink/default.px4board b/boards/sky-drones/smartap-airlink/default.px4board index 3d36f1d99d..fe6411be99 100644 --- a/boards/sky-drones/smartap-airlink/default.px4board +++ b/boards/sky-drones/smartap-airlink/default.px4board @@ -47,6 +47,7 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index de002d7617..a183dd8a95 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -45,6 +45,4 @@ px4_add_module( ecl_yaw_controller.cpp DEPENDS px4_work_queue - RateControl - SlewRate ) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index f1b7084b92..7f7fb5abf4 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,29 +37,16 @@ using namespace time_literals; using namespace matrix; using math::constrain; -using math::interpolate; using math::radians; FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), - _actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)), - _actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)), _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { /* fetch initial parameter values */ parameters_update(); - - // set initial maximum body rate setpoints - _roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get())); - _pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get())); - _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get())); - _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get())); - - _rate_ctrl_status_pub.advertise(); - _spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate); - _flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate); } FixedwingAttitudeControl::~FixedwingAttitudeControl() @@ -78,116 +65,55 @@ FixedwingAttitudeControl::init() return true; } -int +void FixedwingAttitudeControl::parameters_update() { - /* pitch control parameters */ - _pitch_ctrl.set_time_constant(_param_fw_p_tc.get()); - - /* roll control parameters */ _roll_ctrl.set_time_constant(_param_fw_r_tc.get()); + _roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get())); + + _pitch_ctrl.set_time_constant(_param_fw_p_tc.get()); + _pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get())); + _pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get())); + + _yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get())); - /* wheel control parameters */ _wheel_ctrl.set_k_p(_param_fw_wr_p.get()); _wheel_ctrl.set_k_i(_param_fw_wr_i.get()); _wheel_ctrl.set_k_ff(_param_fw_wr_ff.get()); _wheel_ctrl.set_integrator_max(_param_fw_wr_imax.get()); _wheel_ctrl.set_max_rate(radians(_param_fw_w_rmax.get())); - - const Vector3f rate_p = Vector3f(_param_fw_rr_p.get(), _param_fw_pr_p.get(), _param_fw_yr_p.get()); - const Vector3f rate_i = Vector3f(_param_fw_rr_i.get(), _param_fw_pr_i.get(), _param_fw_yr_i.get()); - const Vector3f rate_d = Vector3f(_param_fw_rr_d.get(), _param_fw_pr_d.get(), _param_fw_yr_d.get()); - - _rate_control.setGains(rate_p, rate_i, rate_d); - - _rate_control.setIntegratorLimit( - Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get())); - - _rate_control.setFeedForwardGain( - // set FF gains to 0 as we add the FF control outside of the rate controller - Vector3f(0.f, 0.f, 0.f)); - - - return PX4_OK; -} - -void -FixedwingAttitudeControl::vehicle_control_mode_poll() -{ - _vcontrol_mode_sub.update(&_vcontrol_mode); - - if (_vehicle_status.is_vtol) { - const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && !_vehicle_status.in_transition_mode; - const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _vehicle_status.is_vtol_tailsitter; - - if (is_hovering || is_tailsitter_transition) { - _vcontrol_mode.flag_control_attitude_enabled = false; - _vcontrol_mode.flag_control_manual_enabled = false; - } - } } void FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) { - const bool is_tailsitter_transition = _vehicle_status.is_vtol_tailsitter && _vehicle_status.in_transition_mode; - const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; - - if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) { + if (_vcontrol_mode.flag_control_manual_enabled && _in_fw_or_transition_wo_tailsitter_transition) { // Always copy the new manual setpoint, even if it wasn't updated, to fill the actuators with valid values if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { - if (!_vcontrol_mode.flag_control_climb_rate_enabled) { + if (!_vcontrol_mode.flag_control_climb_rate_enabled & _vcontrol_mode.flag_control_attitude_enabled) { - const float throttle = (_manual_control_setpoint.throttle + 1.f) * .5f; + // STABILIZED mode generate the attitude setpoint from manual user inputs - if (_vcontrol_mode.flag_control_attitude_enabled) { - // STABILIZED mode generate the attitude setpoint from manual user inputs + _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get()); - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get()); + _att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get()) + + radians(_param_fw_psp_off.get()); + _att_sp.pitch_body = constrain(_att_sp.pitch_body, + -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); - _att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get()) - + radians(_param_fw_psp_off.get()); - _att_sp.pitch_body = constrain(_att_sp.pitch_body, - -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); + _att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw + _att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; - _att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw - _att_sp.thrust_body[0] = throttle; + Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + q.copyTo(_att_sp.q_d); - Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); - q.copyTo(_att_sp.q_d); + _att_sp.reset_integral = false; - _att_sp.reset_integral = false; + _att_sp.timestamp = hrt_absolute_time(); - _att_sp.timestamp = hrt_absolute_time(); - - _attitude_sp_pub.publish(_att_sp); - - } else if (_vcontrol_mode.flag_control_rates_enabled && - !_vcontrol_mode.flag_control_attitude_enabled) { - - // RATE mode we need to generate the rate setpoint from manual user inputs - _rates_sp.timestamp = hrt_absolute_time(); - _rates_sp.roll = _manual_control_setpoint.roll * radians(_param_fw_acro_x_max.get()); - _rates_sp.pitch = -_manual_control_setpoint.pitch * radians(_param_fw_acro_y_max.get()); - _rates_sp.yaw = _manual_control_setpoint.yaw * radians(_param_fw_acro_z_max.get()); - _rates_sp.thrust_body[0] = throttle; - - _rate_sp_pub.publish(_rates_sp); - - } else { - /* manual/direct control */ - _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = - _manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get(); - _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = - -_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); - _actuator_controls.control[actuator_controls_s::INDEX_YAW] = - _manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = throttle; - _landing_gear_wheel.normalized_wheel_setpoint = _manual_control_setpoint.yaw; - } + _attitude_sp_pub.publish(_att_sp); } } } @@ -215,7 +141,7 @@ FixedwingAttitudeControl::vehicle_land_detected_poll() } } -float FixedwingAttitudeControl::get_airspeed_and_update_scaling() +float FixedwingAttitudeControl::get_airspeed_constrained() { _airspeed_validated_sub.update(); const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().calibrated_airspeed_m_s) @@ -238,19 +164,7 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling() } } - /* - * For scaling our actuators using anything less than the stall - * speed doesn't make any sense - its the strongest reasonable deflection we - * want to do in flight and it's the baseline a human pilot would choose. - * - * Forcing the scaling to this value allows reasonable handheld tests. - */ - const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_stall.get(), - _param_fw_airspd_max.get()), 0.1f, 1000.0f); - - _airspeed_scaling = (_param_fw_arsp_scale_en.get()) ? (_param_fw_airspd_trim.get() / airspeed_constrained) : 1.0f; - - return airspeed; + return math::constrain(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_max.get()); } void FixedwingAttitudeControl::Run() @@ -267,7 +181,7 @@ void FixedwingAttitudeControl::Run() if (_att_sub.updated() || (hrt_elapsed_time(&_last_run) > 20_ms)) { // only update parameters if they changed - bool params_updated = _parameter_update_sub.updated(); + const bool params_updated = _parameter_update_sub.updated(); // check for parameter updates if (params_updated) { @@ -303,11 +217,7 @@ void FixedwingAttitudeControl::Run() vehicle_angular_velocity_s angular_velocity{}; _vehicle_rates_sub.copy(&angular_velocity); - float rollspeed = angular_velocity.xyz[0]; - float pitchspeed = angular_velocity.xyz[1]; - float yawspeed = angular_velocity.xyz[2]; - const Vector3f rates(rollspeed, pitchspeed, yawspeed); - const Vector3f angular_accel{angular_velocity.xyz_derivative}; + float yawspeed = angular_velocity.xyz[2]; // only used for wheel controller if (_vehicle_status.is_vtol_tailsitter) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode @@ -347,9 +257,7 @@ void FixedwingAttitudeControl::Run() _R = R_adapted; /* lastly, roll- and yawspeed have to be swaped */ - float helper = rollspeed; - rollspeed = -yawspeed; - yawspeed = helper; + yawspeed = angular_velocity.xyz[0]; } const matrix::Eulerf euler_angles(_R); @@ -358,10 +266,15 @@ void FixedwingAttitudeControl::Run() vehicle_attitude_setpoint_poll(); - // vehicle status update must be before the vehicle_control_mode_poll(), otherwise rate sp are not published during whole transition + // vehicle status update must be before the vehicle_control_mode poll, otherwise rate sp are not published during whole transition _vehicle_status_sub.update(&_vehicle_status); + const bool is_in_transition_except_tailsitter = _vehicle_status.in_transition_mode + && !_vehicle_status.is_vtol_tailsitter; + const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + _in_fw_or_transition_wo_tailsitter_transition = is_fixed_wing || is_in_transition_except_tailsitter; + + _vehicle_control_mode_sub.update(&_vcontrol_mode); - vehicle_control_mode_poll(); vehicle_land_detected_poll(); // the position controller will not emit attitude setpoints in some modes @@ -376,26 +289,18 @@ void FixedwingAttitudeControl::Run() /* if we are in rotary wing mode, do nothing */ if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) { - _spoiler_setpoint_with_slewrate.setForcedValue(0.f); - _flaps_setpoint_with_slewrate.setForcedValue(0.f); perf_end(_loop_perf); return; } - controlFlaps(dt); - controlSpoilers(dt); - if (_vcontrol_mode.flag_control_rates_enabled) { - const float airspeed = get_airspeed_and_update_scaling(); - /* Reset integrators if commanded by attitude setpoint, or the aircraft is on ground * or a multicopter (but not transitioning VTOL or tailsitter) */ if (_att_sp.reset_integral || _landed - || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) { + || !_in_fw_or_transition_wo_tailsitter_transition) { _rates_sp.reset_integral = true; _wheel_ctrl.reset_integrator(); @@ -404,26 +309,25 @@ void FixedwingAttitudeControl::Run() _rates_sp.reset_integral = false; } - // update saturation status from control allocation feedback - control_allocator_status_s control_allocator_status; + float groundspeed_scale = 1.f; - if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) { - Vector saturation_positive; - Vector saturation_negative; + if (wheel_control) { + if (_local_pos_sub.updated()) { + vehicle_local_position_s vehicle_local_position; - if (!control_allocator_status.torque_setpoint_achieved) { - for (size_t i = 0; i < 3; i++) { - if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { - saturation_positive(i) = true; - - } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { - saturation_negative(i) = true; - } + if (_local_pos_sub.copy(&vehicle_local_position)) { + _groundspeed = sqrtf(vehicle_local_position.vx * vehicle_local_position.vx + vehicle_local_position.vy * + vehicle_local_position.vy); } } - // TODO: send the unallocated value directly for better anti-windup - _rate_control.setSaturationStatus(saturation_positive, saturation_negative); + // Use stall airspeed to calculate ground speed scaling region. Don't scale below gspd_scaling_trim + float gspd_scaling_trim = (_param_fw_airspd_stall.get()); + + if (_groundspeed > gspd_scaling_trim) { + groundspeed_scale = gspd_scaling_trim / _groundspeed; + + } } /* Prepare data for attitude controllers */ @@ -431,91 +335,19 @@ void FixedwingAttitudeControl::Run() control_input.roll = euler_angles.phi(); control_input.pitch = euler_angles.theta(); control_input.yaw = euler_angles.psi(); - control_input.body_x_rate = rollspeed; - control_input.body_y_rate = pitchspeed; control_input.body_z_rate = yawspeed; control_input.roll_setpoint = _att_sp.roll_body; control_input.pitch_setpoint = _att_sp.pitch_body; control_input.yaw_setpoint = _att_sp.yaw_body; - control_input.euler_roll_rate_setpoint = _roll_ctrl.get_euler_rate_setpoint(); control_input.euler_pitch_rate_setpoint = _pitch_ctrl.get_euler_rate_setpoint(); control_input.euler_yaw_rate_setpoint = _yaw_ctrl.get_euler_rate_setpoint(); - control_input.airspeed_min = _param_fw_airspd_stall.get(); - control_input.airspeed_max = _param_fw_airspd_max.get(); - control_input.airspeed = airspeed; - - if (wheel_control) { - _local_pos_sub.update(&_local_pos); - - /* Use stall airspeed to calculate ground speed scaling region. - * Don't scale below gspd_scaling_trim - */ - float groundspeed = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy); - float gspd_scaling_trim = (_param_fw_airspd_stall.get()); - - control_input.groundspeed = groundspeed; - - if (groundspeed > gspd_scaling_trim) { - control_input.groundspeed_scaler = gspd_scaling_trim / groundspeed; - - } else { - control_input.groundspeed_scaler = 1.0f; - } - } - - /* reset body angular rate limits on mode change */ - if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) { - if (_vcontrol_mode.flag_control_attitude_enabled - || _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get())); - _pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get())); - _pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get())); - _yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get())); - - } else { - _roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get())); - _pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get())); - _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get())); - _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get())); - } - } - - _flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled; - - /* bi-linear interpolation over airspeed for actuator trim scheduling */ - float trim_roll = _param_trim_roll.get(); - float trim_pitch = _param_trim_pitch.get(); - float trim_yaw = _param_trim_yaw.get(); - - if (airspeed < _param_fw_airspd_trim.get()) { - trim_roll += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_r_vmin.get(), - 0.0f); - trim_pitch += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_p_vmin.get(), - 0.0f); - trim_yaw += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_y_vmin.get(), - 0.0f); - - } else { - trim_roll += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_r_vmax.get()); - trim_pitch += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_p_vmax.get()); - trim_yaw += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_y_vmax.get()); - } - - /* add trim increment if flaps are deployed */ - trim_roll += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_r_flps.get(); - trim_pitch += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_flps.get(); - - // add trim increment from spoilers (only pitch) - trim_pitch += _spoiler_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_spoil.get(); + control_input.airspeed_constrained = get_airspeed_constrained(); + control_input.groundspeed = _groundspeed; + control_input.groundspeed_scaler = groundspeed_scale; /* Run attitude controllers */ - if (_vcontrol_mode.flag_control_attitude_enabled) { + + if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) { if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { _roll_ctrl.control_attitude(dt, control_input); _pitch_ctrl.control_attitude(dt, control_input); @@ -532,7 +364,6 @@ void FixedwingAttitudeControl::Run() Vector3f body_rates_setpoint = Vector3f(_roll_ctrl.get_body_rate_setpoint(), _pitch_ctrl.get_body_rate_setpoint(), _yaw_ctrl.get_body_rate_setpoint()); - const hrt_abstime now = hrt_absolute_time(); autotune_attitude_control_status_s pid_autotune; matrix::Vector3f bodyrate_autotune_ff; @@ -541,7 +372,7 @@ void FixedwingAttitudeControl::Run() || pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH || pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW || pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST) - && ((now - pid_autotune.timestamp) < 1_s)) { + && ((hrt_absolute_time() - pid_autotune.timestamp) < 1_s)) { bodyrate_autotune_ff = matrix::Vector3f(pid_autotune.rate_sp); body_rates_setpoint += bodyrate_autotune_ff; @@ -550,7 +381,7 @@ void FixedwingAttitudeControl::Run() /* add yaw rate setpoint from sticks in all attitude-controlled modes */ if (_vcontrol_mode.flag_control_manual_enabled) { - body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.yaw * radians(_param_fw_man_yr_max.get()), + body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.yaw * radians(_param_fw_y_rmax.get()), -radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get())); } @@ -565,124 +396,29 @@ void FixedwingAttitudeControl::Run() } } - if (_vcontrol_mode.flag_control_rates_enabled) { - _rates_sp_sub.update(&_rates_sp); + // wheel control + float wheel_u = 0.f; - const Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw); + if (_vcontrol_mode.flag_control_manual_enabled) { + // always direct control of steering wheel with yaw stick in manual modes + wheel_u = _manual_control_setpoint.yaw; - /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ - const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, - _landed); - - float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0); - float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward; - _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = - (PX4_ISFINITE(roll_u)) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll; - - float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1); - float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward; - _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = - (PX4_ISFINITE(pitch_u)) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch; - - const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2); - const float yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward; - - // wheel control - float wheel_u = 0.f; - - if (_vcontrol_mode.flag_control_manual_enabled) { - // always direct control of steering wheel with yaw stick in manual modes - wheel_u = _manual_control_setpoint.yaw; - - } else { - // XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from - // position controller during auto modes _manual_control_setpoint.yaw gets passed - // whenever nudging is enabled, otherwise zero - wheel_u = wheel_control ? _wheel_ctrl.control_bodyrate(dt, control_input) - + _att_sp.yaw_sp_move_rate : 0.f; - } - - _landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(wheel_u) ? wheel_u : 0.f; - - _actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw, - -1.f, 1.f) : trim_yaw; - - if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u) || _rates_sp.reset_integral) { - _rate_control.resetIntegral(); - } - - if (!PX4_ISFINITE(wheel_u)) { - _wheel_ctrl.reset_integrator(); - } - - /* throttle passed through if it is finite */ - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = - (PX4_ISFINITE(_rates_sp.thrust_body[0])) ? _rates_sp.thrust_body[0] : 0.0f; - - /* scale effort by battery status */ - if (_param_fw_bat_scale_en.get() && - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) { - - if (_battery_status_sub.updated()) { - battery_status_s battery_status{}; - - if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { - _battery_scale = battery_status.scale; - } - } - - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale; - } + } else { + // XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from + // position controller during auto modes _manual_control_setpoint.r gets passed + // whenever nudging is enabled, otherwise zero + wheel_u = wheel_control ? _wheel_ctrl.control_bodyrate(dt, control_input) + + _att_sp.yaw_sp_move_rate : 0.f; } - // publish rate controller status - rate_ctrl_status_s rate_ctrl_status{}; - _rate_control.getRateControlStatus(rate_ctrl_status); - rate_ctrl_status.timestamp = hrt_absolute_time(); - - if (wheel_control) { - rate_ctrl_status.wheel_rate_integ = _wheel_ctrl.get_integrator(); - } - - _rate_ctrl_status_pub.publish(rate_ctrl_status); + _landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(wheel_u) ? wheel_u : 0.f; + _landing_gear_wheel.timestamp = hrt_absolute_time(); + _landing_gear_wheel_pub.publish(_landing_gear_wheel); } else { // full manual - _rate_control.resetIntegral(); _wheel_ctrl.reset_integrator(); } - - // Add feed-forward from roll control output to yaw control output - // This can be used to counteract the adverse yaw effect when rolling the plane - _actuator_controls.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get() - * constrain(_actuator_controls.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); - - _actuator_controls.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState(); - _actuator_controls.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState(); - _actuator_controls.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f; - // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future - _actuator_controls.control[actuator_controls_s::INDEX_LANDING_GEAR] = _manual_control_setpoint.aux3; - - /* lazily publish the setpoint only once available */ - _actuator_controls.timestamp = hrt_absolute_time(); - _actuator_controls.timestamp_sample = att.timestamp; - - /* Only publish if any of the proper modes are enabled */ - if (_vcontrol_mode.flag_control_rates_enabled || - _vcontrol_mode.flag_control_attitude_enabled || - _vcontrol_mode.flag_control_manual_enabled) { - _actuator_controls_0_pub.publish(_actuator_controls); - - if (!_vehicle_status.is_vtol) { - publishTorqueSetpoint(angular_velocity.timestamp_sample); - publishThrustSetpoint(angular_velocity.timestamp_sample); - } - } - - _landing_gear_wheel.timestamp = hrt_absolute_time(); - _landing_gear_wheel_pub.publish(_landing_gear_wheel); - - updateActuatorControlsStatus(dt); } // backup schedule @@ -691,132 +427,6 @@ void FixedwingAttitudeControl::Run() perf_end(_loop_perf); } -void FixedwingAttitudeControl::publishTorqueSetpoint(const hrt_abstime ×tamp_sample) -{ - vehicle_torque_setpoint_s v_torque_sp = {}; - v_torque_sp.timestamp = hrt_absolute_time(); - v_torque_sp.timestamp_sample = timestamp_sample; - v_torque_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_ROLL]; - v_torque_sp.xyz[1] = _actuator_controls.control[actuator_controls_s::INDEX_PITCH]; - v_torque_sp.xyz[2] = _actuator_controls.control[actuator_controls_s::INDEX_YAW]; - - _vehicle_torque_setpoint_pub.publish(v_torque_sp); -} - -void FixedwingAttitudeControl::publishThrustSetpoint(const hrt_abstime ×tamp_sample) -{ - vehicle_thrust_setpoint_s v_thrust_sp = {}; - v_thrust_sp.timestamp = hrt_absolute_time(); - v_thrust_sp.timestamp_sample = timestamp_sample; - v_thrust_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]; - v_thrust_sp.xyz[1] = 0.0f; - v_thrust_sp.xyz[2] = 0.0f; - - _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); -} - -void FixedwingAttitudeControl::controlFlaps(const float dt) -{ - /* default flaps to center */ - float flap_control = 0.0f; - - /* map flaps by default to manual if valid */ - if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled) { - flap_control = _manual_control_setpoint.flaps; - - } else if (_vcontrol_mode.flag_control_auto_enabled) { - - switch (_att_sp.apply_flaps) { - case vehicle_attitude_setpoint_s::FLAPS_OFF: - flap_control = 0.0f; // no flaps - break; - - case vehicle_attitude_setpoint_s::FLAPS_LAND: - flap_control = _param_fw_flaps_lnd_scl.get(); - break; - - case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF: - flap_control = _param_fw_flaps_to_scl.get(); - break; - } - } - - // move the actual control value continuous with time, full flap travel in 1sec - _flaps_setpoint_with_slewrate.update(math::constrain(flap_control, 0.f, 1.f), dt); -} - -void FixedwingAttitudeControl::controlSpoilers(const float dt) -{ - float spoiler_control = 0.f; - - if (_vcontrol_mode.flag_control_manual_enabled) { - switch (_param_fw_spoilers_man.get()) { - case 0: - break; - - case 1: - spoiler_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? _manual_control_setpoint.flaps : 0.f; - break; - - case 2: - spoiler_control = PX4_ISFINITE(_manual_control_setpoint.aux1) ? _manual_control_setpoint.aux1 : 0.f; - break; - } - - } else if (_vcontrol_mode.flag_control_auto_enabled) { - switch (_att_sp.apply_spoilers) { - case vehicle_attitude_setpoint_s::SPOILERS_OFF: - spoiler_control = 0.f; - break; - - case vehicle_attitude_setpoint_s::SPOILERS_LAND: - spoiler_control = _param_fw_spoilers_lnd.get(); - break; - - case vehicle_attitude_setpoint_s::SPOILERS_DESCEND: - spoiler_control = _param_fw_spoilers_desc.get(); - break; - } - } - - _spoiler_setpoint_with_slewrate.update(math::constrain(spoiler_control, 0.f, 1.f), dt); -} - -void FixedwingAttitudeControl::updateActuatorControlsStatus(float dt) -{ - for (int i = 0; i < 4; i++) { - float control_signal; - - if (i <= actuator_controls_status_s::INDEX_YAW) { - // We assume that the attitude is actuated by control surfaces - // consuming power only when they move - control_signal = _actuator_controls.control[i] - _control_prev[i]; - _control_prev[i] = _actuator_controls.control[i]; - - } else { - control_signal = _actuator_controls.control[i]; - } - - _control_energy[i] += control_signal * control_signal * dt; - } - - _energy_integration_time += dt; - - if (_energy_integration_time > 500e-3f) { - - actuator_controls_status_s status; - status.timestamp = _actuator_controls.timestamp; - - for (int i = 0; i < 4; i++) { - status.control_power[i] = _control_energy[i] / _energy_integration_time; - _control_energy[i] = 0.f; - } - - _actuator_controls_status_pub.publish(status); - _energy_integration_time = 0.f; - } -} - int FixedwingAttitudeControl::task_spawn(int argc, char *argv[]) { bool vtol = false; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index ea6f9535f2..63bb8cbd93 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -33,18 +33,14 @@ #pragma once -#include - #include #include "ecl_pitch_controller.h" #include "ecl_roll_controller.h" #include "ecl_wheel_controller.h" #include "ecl_yaw_controller.h" -#include #include #include #include -#include #include #include #include @@ -54,20 +50,13 @@ #include #include #include -#include #include -#include #include -#include -#include #include #include -#include -#include #include #include #include -#include #include #include #include @@ -76,8 +65,6 @@ #include #include #include -#include -#include using matrix::Eulerf; using matrix::Quatf; @@ -86,9 +73,6 @@ using uORB::SubscriptionData; using namespace time_literals; -static constexpr float kFlapSlewRate = 1.f; //minimum time from none to full flap deflection [s] -static constexpr float kSpoilerSlewRate = 1.f; //minimum time from none to full spoiler deflection [s] - class FixedwingAttitudeControl final : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { @@ -110,42 +94,28 @@ public: private: void Run() override; - void publishTorqueSetpoint(const hrt_abstime ×tamp_sample); - void publishThrustSetpoint(const hrt_abstime ×tamp_sample); - uORB::SubscriptionCallbackWorkItem _att_sub{this, ORB_ID(vehicle_attitude)}; /**< vehicle attitude */ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */ uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; - uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */ uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ - uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */ - uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)}; - uORB::SubscriptionMultiArray _control_allocator_status_subs{ORB_ID::control_allocator_status}; - uORB::SubscriptionData _airspeed_validated_sub{ORB_ID(airspeed_validated)}; - uORB::Publication _actuator_controls_0_pub; - uORB::Publication _actuator_controls_status_pub; uORB::Publication _attitude_sp_pub; uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; - uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; - uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; - uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; uORB::Publication _landing_gear_wheel_pub{ORB_ID(landing_gear_wheel)}; - actuator_controls_s _actuator_controls{}; manual_control_setpoint_s _manual_control_setpoint{}; vehicle_attitude_setpoint_s _att_sp{}; vehicle_control_mode_s _vcontrol_mode{}; - vehicle_local_position_s _local_pos{}; vehicle_rates_setpoint_s _rates_sp{}; vehicle_status_s _vehicle_status{}; landing_gear_wheel_s _landing_gear_wheel{}; @@ -156,76 +126,27 @@ private: hrt_abstime _last_run{0}; - float _airspeed_scaling{1.0f}; - bool _landed{true}; - - float _battery_scale{1.0f}; - - bool _flag_control_attitude_enabled_last{false}; - - float _energy_integration_time{0.0f}; - float _control_energy[4] {}; - float _control_prev[3] {}; - - SlewRate _spoiler_setpoint_with_slewrate; - SlewRate _flaps_setpoint_with_slewrate; + float _groundspeed{0.f}; + bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states DEFINE_PARAMETERS( - (ParamFloat) _param_fw_acro_x_max, - (ParamFloat) _param_fw_acro_y_max, - (ParamFloat) _param_fw_acro_z_max, - (ParamFloat) _param_fw_airspd_max, (ParamFloat) _param_fw_airspd_min, (ParamFloat) _param_fw_airspd_stall, (ParamFloat) _param_fw_airspd_trim, (ParamInt) _param_fw_arsp_mode, - (ParamInt) _param_fw_arsp_scale_en, - - (ParamBool) _param_fw_bat_scale_en, - - (ParamFloat) _param_fw_dtrim_p_flps, - (ParamFloat) _param_fw_dtrim_p_spoil, - (ParamFloat) _param_fw_dtrim_p_vmax, - (ParamFloat) _param_fw_dtrim_p_vmin, - (ParamFloat) _param_fw_dtrim_r_flps, - (ParamFloat) _param_fw_dtrim_r_vmax, - (ParamFloat) _param_fw_dtrim_r_vmin, - (ParamFloat) _param_fw_dtrim_y_vmax, - (ParamFloat) _param_fw_dtrim_y_vmin, - - (ParamFloat) _param_fw_flaps_lnd_scl, - (ParamFloat) _param_fw_flaps_to_scl, - (ParamFloat) _param_fw_spoilers_lnd, - (ParamFloat) _param_fw_spoilers_desc, - (ParamInt) _param_fw_spoilers_man, - (ParamFloat) _param_fw_man_p_max, - (ParamFloat) _param_fw_man_p_sc, (ParamFloat) _param_fw_man_r_max, - (ParamFloat) _param_fw_man_r_sc, - (ParamFloat) _param_fw_man_y_sc, (ParamFloat) _param_fw_p_rmax_neg, (ParamFloat) _param_fw_p_rmax_pos, (ParamFloat) _param_fw_p_tc, - (ParamFloat) _param_fw_pr_ff, - (ParamFloat) _param_fw_pr_i, - (ParamFloat) _param_fw_pr_imax, - (ParamFloat) _param_fw_pr_p, - (ParamFloat) _param_fw_pr_d, (ParamFloat) _param_fw_psp_off, (ParamFloat) _param_fw_r_rmax, (ParamFloat) _param_fw_r_tc, - (ParamFloat) _param_fw_rll_to_yaw_ff, - (ParamFloat) _param_fw_rr_ff, - (ParamFloat) _param_fw_rr_i, - (ParamFloat) _param_fw_rr_imax, - (ParamFloat) _param_fw_rr_p, - (ParamFloat) _param_fw_rr_d, (ParamBool) _param_fw_w_en, (ParamFloat) _param_fw_w_rmax, @@ -234,51 +155,17 @@ private: (ParamFloat) _param_fw_wr_imax, (ParamFloat) _param_fw_wr_p, - (ParamFloat) _param_fw_y_rmax, - (ParamFloat) _param_fw_yr_ff, - (ParamFloat) _param_fw_yr_i, - (ParamFloat) _param_fw_yr_imax, - (ParamFloat) _param_fw_yr_p, - (ParamFloat) _param_fw_yr_d, - - (ParamFloat) _param_trim_pitch, - (ParamFloat) _param_trim_roll, - (ParamFloat) _param_trim_yaw, - - (ParamFloat) _param_fw_man_yr_max + (ParamFloat) _param_fw_y_rmax ) - ECL_RollController _roll_ctrl; - ECL_PitchController _pitch_ctrl; - ECL_YawController _yaw_ctrl; - ECL_WheelController _wheel_ctrl; - RateControl _rate_control; ///< class for rate control calculations + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; + ECL_WheelController _wheel_ctrl; - /** - * @brief Update flap control setting - * - * @param dt Current time delta [s] - */ - void controlFlaps(const float dt); - - /** - * @brief Update spoiler control setting - * - * @param dt Current time delta [s] - */ - void controlSpoilers(const float dt); - - void updateActuatorControlsStatus(float dt); - - /** - * Update our local parameter cache. - */ - int parameters_update(); - - void vehicle_control_mode_poll(); - void vehicle_manual_poll(const float yaw_body); - void vehicle_attitude_setpoint_poll(); - void vehicle_land_detected_poll(); - - float get_airspeed_and_update_scaling(); + void parameters_update(); + void vehicle_manual_poll(const float yaw_body); + void vehicle_attitude_setpoint_poll(); + void vehicle_land_detected_poll(); + float get_airspeed_constrained(); }; diff --git a/src/modules/fw_att_control/ecl_controller.cpp b/src/modules/fw_att_control/ecl_controller.cpp index 9fec882ea4..50b6ff3340 100644 --- a/src/modules/fw_att_control/ecl_controller.cpp +++ b/src/modules/fw_att_control/ecl_controller.cpp @@ -117,18 +117,3 @@ float ECL_Controller::get_integrator() { return _integrator; } - -float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) -{ - float airspeed_result = airspeed; - - if (!PX4_ISFINITE(airspeed)) { - /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed_result = 0.5f * (minspeed + maxspeed); - - } else if (airspeed < minspeed) { - airspeed_result = minspeed; - } - - return airspeed_result; -} diff --git a/src/modules/fw_att_control/ecl_controller.h b/src/modules/fw_att_control/ecl_controller.h index a32a922836..0e58cfec5b 100644 --- a/src/modules/fw_att_control/ecl_controller.h +++ b/src/modules/fw_att_control/ecl_controller.h @@ -55,18 +55,13 @@ struct ECL_ControlData { float roll; float pitch; float yaw; - float body_x_rate; - float body_y_rate; float body_z_rate; float roll_setpoint; float pitch_setpoint; float yaw_setpoint; - float euler_roll_rate_setpoint; float euler_pitch_rate_setpoint; float euler_yaw_rate_setpoint; - float airspeed_min; - float airspeed_max; - float airspeed; + float airspeed_constrained; float groundspeed; float groundspeed_scaler; }; @@ -113,5 +108,4 @@ protected: float _integrator; float _euler_rate_setpoint; float _body_rate_setpoint; - float constrain_airspeed(float airspeed, float minspeed, float maxspeed); }; diff --git a/src/modules/fw_att_control/ecl_pitch_controller.cpp b/src/modules/fw_att_control/ecl_pitch_controller.cpp index 34e07a8153..2518a4a5ff 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.cpp +++ b/src/modules/fw_att_control/ecl_pitch_controller.cpp @@ -49,8 +49,7 @@ float ECL_PitchController::control_attitude(const float dt, const ECL_ControlDat if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) && PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed))) { + PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint))) { return _body_rate_setpoint; } diff --git a/src/modules/fw_att_control/ecl_yaw_controller.cpp b/src/modules/fw_att_control/ecl_yaw_controller.cpp index fdb0878d46..f49b1c9edd 100644 --- a/src/modules/fw_att_control/ecl_yaw_controller.cpp +++ b/src/modules/fw_att_control/ecl_yaw_controller.cpp @@ -48,7 +48,8 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData /* Do not calculate control signal with bad inputs */ if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.euler_pitch_rate_setpoint))) { + PX4_ISFINITE(ctl_data.euler_pitch_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_constrained))) { return _body_rate_setpoint; } @@ -81,8 +82,7 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData if (!inverted) { /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ - _euler_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed < - ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed); + _euler_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / ctl_data.airspeed_constrained; /* Transform setpoint to body angular rates (jacobian) */ const float yaw_body_rate_setpoint_raw = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint + diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index c43a678564..0c22c02e76 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -81,49 +81,6 @@ PARAM_DEFINE_FLOAT(FW_R_TC, 0.4f); */ PARAM_DEFINE_FLOAT(FW_P_TC, 0.4f); -/** - * Pitch rate proportional gain. - * - * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. - * - * @unit %/rad/s - * @min 0.0 - * @max 2.0 - * @decimal 3 - * @increment 0.005 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f); - -/** - * Pitch rate derivative gain. - * - * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. - * - * @unit %/rad/s - * @min 0.0 - * @max 1.0 - * @decimal 3 - * @increment 0.005 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_PR_D, 0.f); - -/** - * Pitch rate integrator gain. - * - * This gain defines how much control response will result out of a steady - * state error. It trims any constant error. - * - * @unit %/rad - * @min 0.0 - * @max 1 - * @decimal 3 - * @increment 0.005 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_PR_I, 0.1f); - /** * Maximum positive / up pitch rate. * @@ -154,76 +111,6 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f); */ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f); -/** - * Pitch rate integrator limit - * - * The portion of the integrator part in the control surface deflection is - * limited to this value - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); - -/** - * Roll rate proportional Gain - * - * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. - * - * @unit %/rad/s - * @min 0.0 - * @max 2.0 - * @decimal 3 - * @increment 0.005 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); - -/** - * Roll rate derivative Gain - * - * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. - * - * @unit %/rad/s - * @min 0.0 - * @max 1.0 - * @decimal 3 - * @increment 0.005 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_RR_D, 0.00f); - -/** - * Roll rate integrator Gain - * - * This gain defines how much control response will result out of a steady - * state error. It trims any constant error. - * - * @unit %/rad - * @min 0.0 - * @max 1 - * @decimal 3 - * @increment 0.005 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_RR_I, 0.1f); - -/** - * Roll integrator anti-windup - * - * The portion of the integrator part in the control surface deflection is limited to this value. - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.4f); - /** * Maximum roll rate * @@ -239,63 +126,6 @@ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.4f); */ PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f); -/** - * Yaw rate proportional gain - * - * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. - * - * @unit %/rad/s - * @min 0.0 - * @max 2.0 - * @decimal 3 - * @increment 0.005 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f); - -/** - * Yaw rate derivative gain - * - * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. - * - * @unit %/rad/s - * @min 0.0 - * @max 1.0 - * @decimal 3 - * @increment 0.005 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_YR_D, 0.0f); - -/** - * Yaw rate integrator gain - * - * This gain defines how much control response will result out of a steady - * state error. It trims any constant error. - * - * @unit %/rad - * @min 0.0 - * @max 1 - * @decimal 1 - * @increment 0.5 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_YR_I, 0.1f); - -/** - * Yaw rate integrator limit - * - * The portion of the integrator part in the control surface deflection is - * limited to this value - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.4f); - /** * Maximum yaw rate * @@ -311,21 +141,6 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.4f); */ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 50.0f); -/** - * Roll control to yaw control feedforward gain. - * - * This gain can be used to counteract the "adverse yaw" effect for fixed wings. - * When the plane enters a roll it will tend to yaw the nose out of the turn. - * This gain enables the use of a yaw actuator (rudder, airbrakes, ...) to counteract - * this effect. - * - * @min 0.0 - * @decimal 1 - * @increment 0.01 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f); - /** * Enable wheel steering controller * @@ -397,50 +212,6 @@ PARAM_DEFINE_FLOAT(FW_WR_IMAX, 0.4f); */ PARAM_DEFINE_FLOAT(FW_W_RMAX, 30.0f); -/** - * Roll rate feed forward - * - * Direct feed forward from rate setpoint to control surface output. Use this - * to obtain a tigher response of the controller without introducing - * noise amplification. - * - * @unit %/rad/s - * @min 0.0 - * @max 1 - * @decimal 2 - * @increment 0.05 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f); - -/** - * Pitch rate feed forward - * - * Direct feed forward from rate setpoint to control surface output - * - * @unit %/rad/s - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); - -/** - * Yaw rate feed forward - * - * Direct feed forward from rate setpoint to control surface output - * - * @unit %/rad/s - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); - /** * Wheel steering rate feed forward * @@ -471,344 +242,6 @@ PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f); */ PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); -/** - * Maximum manual roll angle - * - * Maximum manual roll angle setpoint (positive & negative) in manual attitude-only stabilized mode - * - * @unit deg - * @min 0.0 - * @max 90.0 - * @decimal 1 - * @increment 0.5 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); - -/** - * Maximum manual pitch angle - * - * Maximum manual pitch angle setpoint (positive & negative) in manual attitude-only stabilized mode - * - * @unit deg - * @min 0.0 - * @max 90.0 - * @decimal 1 - * @increment 0.5 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 30.0f); - -/** - * Flaps setting during take-off - * - * Sets a fraction of full flaps during take-off. - * - * @unit norm - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.01 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f); - -/** - * Flaps setting during landing - * - * Sets a fraction of full flaps during landing. - * - * @unit norm - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.01 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f); - -/** - * Airspeed mode - * - * For small wings or VTOL without airspeed sensor this parameter can be used to - * enable flying without an airspeed reading - * - * @value 0 Normal (use airspeed if available) - * @value 1 Airspeed disabled - * @group FW Attitude Control - */ -PARAM_DEFINE_INT32(FW_ARSP_MODE, 0); - -/** - * Enable airspeed scaling - * - * This enables a logic that automatically adjusts the output of the rate controller to take - * into account the real torque produced by an aerodynamic control surface given - * the current deviation from the trim airspeed (FW_AIRSPD_TRIM). - * - * Enable when using aerodynamic control surfaces (e.g.: plane) - * Disable when using rotor wings (e.g.: autogyro) - * - * @boolean - * @group FW Attitude Control - */ -PARAM_DEFINE_INT32(FW_ARSP_SCALE_EN, 1); - -/** - * Manual roll scale - * - * Scale factor applied to the desired roll actuator command in full manual mode. This parameter allows - * to adjust the throws of the control surfaces. - * - * @unit norm - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.01 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_MAN_R_SC, 1.0f); - -/** - * Manual pitch scale - * - * Scale factor applied to the desired pitch actuator command in full manual mode. This parameter allows - * to adjust the throws of the control surfaces. - * - * @unit norm - * @min 0.0 - * @decimal 2 - * @increment 0.01 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f); - -/** - * Manual yaw scale - * - * Scale factor applied to the desired yaw actuator command in full manual mode. This parameter allows - * to adjust the throws of the control surfaces. - * - * @unit norm - * @min 0.0 - * @decimal 2 - * @increment 0.01 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f); - -/** - * Whether to scale throttle by battery power level - * - * This compensates for voltage drop of the battery over time by attempting to - * normalize performance across the operating range of the battery. The fixed wing - * should constantly behave as if it was fully charged with reduced max thrust - * at lower battery percentages. i.e. if cruise speed is at 0.5 throttle at 100% battery, - * it will still be 0.5 at 60% battery. - * - * @boolean - * @group FW Attitude Control - */ -PARAM_DEFINE_INT32(FW_BAT_SCALE_EN, 0); - -/** - * Acro body x max rate. - * - * This is the rate the controller is trying to achieve if the user applies full roll - * stick input in acro mode. - * - * @min 45 - * @max 720 - * @decimal 1 - * @increment 5 - * @unit deg/s - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90.f); - -/** - * Acro body y max rate. - * - * This is the body y rate the controller is trying to achieve if the user applies full pitch - * stick input in acro mode. - * - * @min 45 - * @max 720 - * @decimal 1 - * @increment 5 - * @unit deg/s - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90.f); - -/** - * Acro body z max rate. - * - * This is the body z rate the controller is trying to achieve if the user applies full yaw - * stick input in acro mode. - * - * @min 10 - * @max 180 - * @decimal 1 - * @increment 5 - * @unit deg/s - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45.f); - -/** -* Roll trim increment at minimum airspeed -* -* This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MIN. - * - * @group FW Attitude Control - * @min -0.25 - * @max 0.25 - * @decimal 2 - * @increment 0.01 - */ -PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMIN, 0.0f); - -/** -* Pitch trim increment at minimum airspeed -* -* This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MIN. - * - * @group FW Attitude Control - * @min -0.25 - * @max 0.25 - * @decimal 2 - * @increment 0.01 - */ -PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMIN, 0.0f); - -/** -* Yaw trim increment at minimum airspeed -* -* This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MIN. - * - * @group FW Attitude Control - * @min -0.25 - * @max 0.25 - * @decimal 2 - * @increment 0.01 - */ -PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMIN, 0.0f); - -/** -* Roll trim increment at maximum airspeed -* -* This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MAX. - * - * @group FW Attitude Control - * @min -0.25 - * @max 0.25 - * @decimal 2 - * @increment 0.01 - */ -PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMAX, 0.0f); - -/** -* Pitch trim increment at maximum airspeed -* -* This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MAX. - * - * @group FW Attitude Control - * @min -0.25 - * @max 0.25 - * @decimal 2 - * @increment 0.01 - */ -PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMAX, 0.0f); - -/** -* Yaw trim increment at maximum airspeed -* -* This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MAX. - * - * @group FW Attitude Control - * @min -0.25 - * @max 0.25 - * @decimal 2 - * @increment 0.01 - */ -PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMAX, 0.0f); - -/** - * Roll trim increment for flaps configuration - * - * This increment is added to TRIM_ROLL whenever flaps are fully deployed. - * - * @group FW Attitude Control - * @min -0.25 - * @max 0.25 - * @decimal 2 - * @increment 0.01 - */ -PARAM_DEFINE_FLOAT(FW_DTRIM_R_FLPS, 0.0f); - -/** - * Pitch trim increment for flaps configuration - * - * This increment is added to the pitch trim whenever flaps are fully deployed. - * - * @group FW Attitude Control - * @min -0.25 - * @max 0.25 - * @decimal 2 - * @increment 0.01 - */ -PARAM_DEFINE_FLOAT(FW_DTRIM_P_FLPS, 0.0f); - -/** - * Pitch trim increment for spoiler configuration - * - * This increment is added to the pitch trim whenever spoilers are fully deployed. - * - * @group FW Attitude Control - * @min -0.25 - * @max 0.25 - * @decimal 2 - * @increment 0.01 - */ -PARAM_DEFINE_FLOAT(FW_DTRIM_P_SPOIL, 0.f); - -/** - * Spoiler landing setting - * - * @unit norm - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.01 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f); - -/** - * Spoiler descend setting - * - * @unit norm - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.01 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f); - -/** - * Spoiler input in manual flight - * - * Chose source for manual setting of spoilers in manual flight modes. - * - * @value 0 Disabled - * @value 1 Flaps channel - * @value 2 Aux1 - * @group FW Attitude Control - */ -PARAM_DEFINE_INT32(FW_SPOILERS_MAN, 0); - /** * Maximum manually added yaw rate * diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 769efed070..f166c95dce 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -2183,11 +2183,6 @@ FixedwingPositionControl::Run() } _position_setpoint_current_valid = valid_setpoint; - - if (!valid_setpoint) { - events::send(events::ID("fixedwing_position_control_invalid_offboard_sp"), events::Log::Error, - "Invalid offboard setpoint"); - } } } else { diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index ea92e3b0f3..650d1239a0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -514,75 +514,6 @@ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f); * */ - -/** - * Minimum Airspeed (CAS) - * - * The minimal airspeed (calibrated airspeed) the user is able to command. - * Further, if the airspeed falls below this value, the TECS controller will try to - * increase airspeed more aggressively. - * Has to be set according to the vehicle's stall speed (which should be set in FW_AIRSPD_STALL), - * with some margin between the stall speed and minimum airspeed. - * This value corresponds to the desired minimum speed with the default load factor (level flight, default weight), - * and is automatically adpated to the current load factor (calculated from roll setpoint and WEIGHT_GROSS/WEIGHT_BASE). - * - * @unit m/s - * @min 0.5 - * @max 40 - * @decimal 1 - * @increment 0.5 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f); - -/** - * Maximum Airspeed (CAS) - * - * The maximal airspeed (calibrated airspeed) the user is able to command. - * Further, if the airspeed is above this value, the TECS controller will try to decrease - * airspeed more aggressively. - * - * @unit m/s - * @min 0.5 - * @max 40 - * @decimal 1 - * @increment 0.5 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f); - -/** - * Cruise Airspeed (CAS) - * - * The trim CAS (calibrated airspeed) of the vehicle. If an airspeed controller is active, - * this is the default airspeed setpoint that the controller will try to achieve if - * no other airspeed setpoint sources are present (e.g. through non-centered RC sticks). - * - * @unit m/s - * @min 0.5 - * @max 40 - * @decimal 1 - * @increment 0.5 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f); - -/** - * Stall Airspeed (CAS) - * - * The stall airspeed (calibrated airspeed) of the vehicle. - * It is used for airspeed sensor failure detection and for the control - * surface scaling airspeed limits. - * - * @unit m/s - * @min 0.5 - * @max 40 - * @decimal 1 - * @increment 0.5 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_STALL, 7.0f); - /** * Maximum climb rate * diff --git a/src/modules/fw_rate_control/CMakeLists.txt b/src/modules/fw_rate_control/CMakeLists.txt new file mode 100644 index 0000000000..595c8314af --- /dev/null +++ b/src/modules/fw_rate_control/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__fw_rate_control + MAIN fw_rate_control + SRCS + FixedwingRateControl.cpp + FixedwingRateControl.hpp + DEPENDS + px4_work_queue + RateControl + SlewRate + ) diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp new file mode 100644 index 0000000000..e3cf88a84c --- /dev/null +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -0,0 +1,624 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "FixedwingRateControl.hpp" + +using namespace time_literals; +using namespace matrix; + +using math::constrain; +using math::interpolate; +using math::radians; + +FixedwingRateControl::FixedwingRateControl(bool vtol) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)), + _actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + /* fetch initial parameter values */ + parameters_update(); + + _rate_ctrl_status_pub.advertise(); + _spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate); + _flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate); +} + +FixedwingRateControl::~FixedwingRateControl() +{ + perf_free(_loop_perf); +} + +bool +FixedwingRateControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +int +FixedwingRateControl::parameters_update() +{ + const Vector3f rate_p = Vector3f(_param_fw_rr_p.get(), _param_fw_pr_p.get(), _param_fw_yr_p.get()); + const Vector3f rate_i = Vector3f(_param_fw_rr_i.get(), _param_fw_pr_i.get(), _param_fw_yr_i.get()); + const Vector3f rate_d = Vector3f(_param_fw_rr_d.get(), _param_fw_pr_d.get(), _param_fw_yr_d.get()); + + _rate_control.setGains(rate_p, rate_i, rate_d); + + _rate_control.setIntegratorLimit( + Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get())); + + _rate_control.setFeedForwardGain( + // set FF gains to 0 as we add the FF control outside of the rate controller + Vector3f(0.f, 0.f, 0.f)); + + + return PX4_OK; +} + +void +FixedwingRateControl::vehicle_manual_poll() +{ + if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_climb_rate_enabled + && _in_fw_or_transition_wo_tailsitter_transition) { + + // Always copy the new manual setpoint, even if it wasn't updated, to fill the actuators with valid values + if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { + + if (_vcontrol_mode.flag_control_rates_enabled && + !_vcontrol_mode.flag_control_attitude_enabled) { + + // RATE mode we need to generate the rate setpoint from manual user inputs + _rates_sp.timestamp = hrt_absolute_time(); + _rates_sp.roll = _manual_control_setpoint.roll * radians(_param_fw_acro_x_max.get()); + _rates_sp.pitch = -_manual_control_setpoint.pitch * radians(_param_fw_acro_y_max.get()); + _rates_sp.yaw = _manual_control_setpoint.yaw * radians(_param_fw_acro_z_max.get()); + _rates_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; + + _rate_sp_pub.publish(_rates_sp); + + } else { + /* manual/direct control */ + _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = + _manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get(); + _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = + -_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); + _actuator_controls.control[actuator_controls_s::INDEX_YAW] = + _manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); + _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = (_manual_control_setpoint.throttle + 1.f) * .5f; + } + } + + } +} + +void +FixedwingRateControl::vehicle_land_detected_poll() +{ + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected {}; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + } + } +} + +float FixedwingRateControl::get_airspeed_and_update_scaling() +{ + _airspeed_validated_sub.update(); + const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().calibrated_airspeed_m_s) + && (hrt_elapsed_time(&_airspeed_validated_sub.get().timestamp) < 1_s); + + // if no airspeed measurement is available out best guess is to use the trim airspeed + float airspeed = _param_fw_airspd_trim.get(); + + if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) { + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed_validated_sub.get().calibrated_airspeed_m_s); + + } else { + // VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible + // this assumption is good as long as the vehicle is not hovering in a headwind which is much larger + // than the stall airspeed + if (_vehicle_status.is_vtol && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode) { + airspeed = _param_fw_airspd_stall.get(); + } + } + + /* + * For scaling our actuators using anything less than the stall + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and it's the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_stall.get(), + _param_fw_airspd_max.get()), 0.1f, 1000.0f); + + _airspeed_scaling = (_param_fw_arsp_scale_en.get()) ? (_param_fw_airspd_trim.get() / airspeed_constrained) : 1.0f; + + return airspeed; +} + +void FixedwingRateControl::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // only run controller if angular velocity changed + if (_vehicle_angular_velocity_sub.updated() || (hrt_elapsed_time(&_last_run) > 20_ms)) { //TODO rate! + + // only update parameters if they changed + bool params_updated = _parameter_update_sub.updated(); + + // check for parameter updates + if (params_updated) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + parameters_update(); + } + + float dt = 0.f; + + static constexpr float DT_MIN = 0.002f; + static constexpr float DT_MAX = 0.04f; + + vehicle_angular_velocity_s vehicle_angular_velocity{}; + + if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) { + dt = math::constrain((vehicle_angular_velocity.timestamp_sample - _last_run) * 1e-6f, DT_MIN, DT_MAX); + _last_run = vehicle_angular_velocity.timestamp_sample; + } + + if (dt < DT_MIN || dt > DT_MAX) { + const hrt_abstime time_now_us = hrt_absolute_time(); + dt = math::constrain((time_now_us - _last_run) * 1e-6f, DT_MIN, DT_MAX); + _last_run = time_now_us; + } + + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&angular_velocity); + float rollspeed = angular_velocity.xyz[0]; + float pitchspeed = angular_velocity.xyz[1]; + float yawspeed = angular_velocity.xyz[2]; + const Vector3f rates(rollspeed, pitchspeed, yawspeed); + const Vector3f angular_accel{angular_velocity.xyz_derivative}; + + if (_vehicle_status.is_vtol_tailsitter) { + + /* roll- and yawspeed have to be swaped */ + float helper = rollspeed; + rollspeed = -yawspeed; + yawspeed = helper; + } + + + // this is only to pass through flaps/spoiler setpoints, can be removed once flaps/spoilers + // are handled outside of attitude/rate controller. + // TODO remove it + _att_sp_sub.update(&_att_sp); + + // vehicle status update must be before the vehicle_control_mode poll, otherwise rate sp are not published during whole transition + _vehicle_status_sub.update(&_vehicle_status); + const bool is_in_transition_except_tailsitter = _vehicle_status.in_transition_mode + && !_vehicle_status.is_vtol_tailsitter; + const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + _in_fw_or_transition_wo_tailsitter_transition = is_fixed_wing || is_in_transition_except_tailsitter; + + _vehicle_control_mode_sub.update(&_vcontrol_mode); + + vehicle_land_detected_poll(); + + vehicle_manual_poll(); + vehicle_land_detected_poll(); + + /* if we are in rotary wing mode, do nothing */ + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) { + _spoiler_setpoint_with_slewrate.setForcedValue(0.f); + _flaps_setpoint_with_slewrate.setForcedValue(0.f); + perf_end(_loop_perf); + return; + } + + controlFlaps(dt); + controlSpoilers(dt); + + if (_vcontrol_mode.flag_control_rates_enabled) { + + const float airspeed = get_airspeed_and_update_scaling(); + + /* reset integrals where needed */ + if (_rates_sp.reset_integral) { + _rate_control.resetIntegral(); + } + + // Reset integrators if the aircraft is on ground or not in a state where the fw attitude controller is run + if (_landed || !_in_fw_or_transition_wo_tailsitter_transition) { + + _rate_control.resetIntegral(); + } + + // update saturation status from control allocation feedback + control_allocator_status_s control_allocator_status; + + if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; + + if (!control_allocator_status.torque_setpoint_achieved) { + for (size_t i = 0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; + + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; + } + } + } + + // TODO: send the unallocated value directly for better anti-windup + _rate_control.setSaturationStatus(saturation_positive, saturation_negative); + } + + /* bi-linear interpolation over airspeed for actuator trim scheduling */ + float trim_roll = _param_trim_roll.get(); + float trim_pitch = _param_trim_pitch.get(); + float trim_yaw = _param_trim_yaw.get(); + + if (airspeed < _param_fw_airspd_trim.get()) { + trim_roll += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_r_vmin.get(), + 0.0f); + trim_pitch += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_p_vmin.get(), + 0.0f); + trim_yaw += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_y_vmin.get(), + 0.0f); + + } else { + trim_roll += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_r_vmax.get()); + trim_pitch += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_p_vmax.get()); + trim_yaw += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_y_vmax.get()); + } + + /* add trim increment if flaps are deployed */ + trim_roll += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_r_flps.get(); + trim_pitch += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_flps.get(); + + // add trim increment from spoilers (only pitch) + trim_pitch += _spoiler_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_spoil.get(); + + if (_vcontrol_mode.flag_control_rates_enabled) { + _rates_sp_sub.update(&_rates_sp); + + const Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw); + + /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ + const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, + _landed); + + float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0); + float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward; + _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = + (PX4_ISFINITE(roll_u)) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll; + + float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1); + float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward; + _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = + (PX4_ISFINITE(pitch_u)) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch; + + const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2); + float yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward; + + _actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw, + -1.f, 1.f) : trim_yaw; + + if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) { + _rate_control.resetIntegral(); + } + + /* throttle passed through if it is finite */ + _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = + (PX4_ISFINITE(_rates_sp.thrust_body[0])) ? _rates_sp.thrust_body[0] : 0.0f; + + /* scale effort by battery status */ + if (_param_fw_bat_scale_en.get() && + _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) { + + if (_battery_status_sub.updated()) { + battery_status_s battery_status{}; + + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_scale = battery_status.scale; + } + } + + _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale; + } + } + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + _rate_control.getRateControlStatus(rate_ctrl_status); + rate_ctrl_status.timestamp = hrt_absolute_time(); + + _rate_ctrl_status_pub.publish(rate_ctrl_status); + + } else { + // full manual + _rate_control.resetIntegral(); + } + + // Add feed-forward from roll control output to yaw control output + // This can be used to counteract the adverse yaw effect when rolling the plane + _actuator_controls.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get() + * constrain(_actuator_controls.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); + + _actuator_controls.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState(); + _actuator_controls.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState(); + _actuator_controls.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f; + // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future + _actuator_controls.control[actuator_controls_s::INDEX_LANDING_GEAR] = _manual_control_setpoint.aux3; + + /* lazily publish the setpoint only once available */ + _actuator_controls.timestamp = hrt_absolute_time(); + _actuator_controls.timestamp_sample = vehicle_angular_velocity.timestamp; + + /* Only publish if any of the proper modes are enabled */ + if (_vcontrol_mode.flag_control_rates_enabled || + _vcontrol_mode.flag_control_attitude_enabled || + _vcontrol_mode.flag_control_manual_enabled) { + _actuator_controls_0_pub.publish(_actuator_controls); + + if (!_vehicle_status.is_vtol) { + publishTorqueSetpoint(angular_velocity.timestamp_sample); + publishThrustSetpoint(angular_velocity.timestamp_sample); + } + } + + updateActuatorControlsStatus(dt); + } + + // backup schedule + ScheduleDelayed(20_ms); + + perf_end(_loop_perf); +} + +void FixedwingRateControl::publishTorqueSetpoint(const hrt_abstime ×tamp_sample) +{ + vehicle_torque_setpoint_s v_torque_sp = {}; + v_torque_sp.timestamp = hrt_absolute_time(); + v_torque_sp.timestamp_sample = timestamp_sample; + v_torque_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_ROLL]; + v_torque_sp.xyz[1] = _actuator_controls.control[actuator_controls_s::INDEX_PITCH]; + v_torque_sp.xyz[2] = _actuator_controls.control[actuator_controls_s::INDEX_YAW]; + + _vehicle_torque_setpoint_pub.publish(v_torque_sp); +} + +void FixedwingRateControl::publishThrustSetpoint(const hrt_abstime ×tamp_sample) +{ + vehicle_thrust_setpoint_s v_thrust_sp = {}; + v_thrust_sp.timestamp = hrt_absolute_time(); + v_thrust_sp.timestamp_sample = timestamp_sample; + v_thrust_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]; + v_thrust_sp.xyz[1] = 0.0f; + v_thrust_sp.xyz[2] = 0.0f; + + _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); +} + +void FixedwingRateControl::controlFlaps(const float dt) +{ + /* default flaps to center */ + float flap_control = 0.0f; + + /* map flaps by default to manual if valid */ + if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled) { + flap_control = _manual_control_setpoint.flaps; + + } else if (_vcontrol_mode.flag_control_auto_enabled) { + + switch (_att_sp.apply_flaps) { + case vehicle_attitude_setpoint_s::FLAPS_OFF: + flap_control = 0.0f; // no flaps + break; + + case vehicle_attitude_setpoint_s::FLAPS_LAND: + flap_control = _param_fw_flaps_lnd_scl.get(); + break; + + case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF: + flap_control = _param_fw_flaps_to_scl.get(); + break; + } + } + + // move the actual control value continuous with time, full flap travel in 1sec + _flaps_setpoint_with_slewrate.update(math::constrain(flap_control, 0.f, 1.f), dt); +} + +void FixedwingRateControl::controlSpoilers(const float dt) +{ + float spoiler_control = 0.f; + + if (_vcontrol_mode.flag_control_manual_enabled) { + switch (_param_fw_spoilers_man.get()) { + case 0: + break; + + case 1: + spoiler_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? _manual_control_setpoint.flaps : 0.f; + break; + + case 2: + spoiler_control = PX4_ISFINITE(_manual_control_setpoint.aux1) ? _manual_control_setpoint.aux1 : 0.f; + break; + } + + } else if (_vcontrol_mode.flag_control_auto_enabled) { + switch (_att_sp.apply_spoilers) { + case vehicle_attitude_setpoint_s::SPOILERS_OFF: + spoiler_control = 0.f; + break; + + case vehicle_attitude_setpoint_s::SPOILERS_LAND: + spoiler_control = _param_fw_spoilers_lnd.get(); + break; + + case vehicle_attitude_setpoint_s::SPOILERS_DESCEND: + spoiler_control = _param_fw_spoilers_desc.get(); + break; + } + } + + _spoiler_setpoint_with_slewrate.update(math::constrain(spoiler_control, 0.f, 1.f), dt); +} + +void FixedwingRateControl::updateActuatorControlsStatus(float dt) +{ + for (int i = 0; i < 4; i++) { + float control_signal; + + if (i <= actuator_controls_status_s::INDEX_YAW) { + // We assume that the attitude is actuated by control surfaces + // consuming power only when they move + control_signal = _actuator_controls.control[i] - _control_prev[i]; + _control_prev[i] = _actuator_controls.control[i]; + + } else { + control_signal = _actuator_controls.control[i]; + } + + _control_energy[i] += control_signal * control_signal * dt; + } + + _energy_integration_time += dt; + + if (_energy_integration_time > 500e-3f) { + + actuator_controls_status_s status; + status.timestamp = _actuator_controls.timestamp; + + for (int i = 0; i < 4; i++) { + status.control_power[i] = _control_energy[i] / _energy_integration_time; + _control_energy[i] = 0.f; + } + + _actuator_controls_status_pub.publish(status); + _energy_integration_time = 0.f; + } +} + +int FixedwingRateControl::task_spawn(int argc, char *argv[]) +{ + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + FixedwingRateControl *instance = new FixedwingRateControl(vtol); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int FixedwingRateControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int FixedwingRateControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +fw_rate_control is the fixed-wing rate controller. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("fw_rate_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int fw_rate_control_main(int argc, char *argv[]) +{ + return FixedwingRateControl::main(argc, argv); +} diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp new file mode 100644 index 0000000000..d34148bbbe --- /dev/null +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -0,0 +1,245 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using matrix::Eulerf; +using matrix::Quatf; + +using uORB::SubscriptionData; + +using namespace time_literals; + +static constexpr float kFlapSlewRate = 1.f; //minimum time from none to full flap deflection [s] +static constexpr float kSpoilerSlewRate = 1.f; //minimum time from none to full spoiler deflection [s] + +class FixedwingRateControl final : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + FixedwingRateControl(bool vtol = false); + ~FixedwingRateControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + void publishTorqueSetpoint(const hrt_abstime ×tamp_sample); + void publishThrustSetpoint(const hrt_abstime ×tamp_sample); + + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)}; + + uORB::SubscriptionMultiArray _control_allocator_status_subs{ORB_ID::control_allocator_status}; + + uORB::SubscriptionData _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + + uORB::Publication _actuator_controls_0_pub; + uORB::Publication _actuator_controls_status_pub; + uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; + uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; + + actuator_controls_s _actuator_controls{}; + manual_control_setpoint_s _manual_control_setpoint{}; + vehicle_attitude_setpoint_s _att_sp{}; + vehicle_control_mode_s _vcontrol_mode{}; + vehicle_rates_setpoint_s _rates_sp{}; + vehicle_status_s _vehicle_status{}; + + perf_counter_t _loop_perf; + + hrt_abstime _last_run{0}; + + float _airspeed_scaling{1.0f}; + + bool _landed{true}; + + float _battery_scale{1.0f}; + + float _energy_integration_time{0.0f}; + float _control_energy[4] {}; + float _control_prev[3] {}; + + SlewRate _spoiler_setpoint_with_slewrate; + SlewRate _flaps_setpoint_with_slewrate; + + bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states + + DEFINE_PARAMETERS( + (ParamFloat) _param_fw_acro_x_max, + (ParamFloat) _param_fw_acro_y_max, + (ParamFloat) _param_fw_acro_z_max, + + (ParamFloat) _param_fw_airspd_max, + (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_airspd_stall, + (ParamFloat) _param_fw_airspd_trim, + (ParamInt) _param_fw_arsp_mode, + + (ParamInt) _param_fw_arsp_scale_en, + + (ParamBool) _param_fw_bat_scale_en, + + (ParamFloat) _param_fw_dtrim_p_flps, + (ParamFloat) _param_fw_dtrim_p_spoil, + (ParamFloat) _param_fw_dtrim_p_vmax, + (ParamFloat) _param_fw_dtrim_p_vmin, + (ParamFloat) _param_fw_dtrim_r_flps, + (ParamFloat) _param_fw_dtrim_r_vmax, + (ParamFloat) _param_fw_dtrim_r_vmin, + (ParamFloat) _param_fw_dtrim_y_vmax, + (ParamFloat) _param_fw_dtrim_y_vmin, + + (ParamFloat) _param_fw_flaps_lnd_scl, + (ParamFloat) _param_fw_flaps_to_scl, + (ParamFloat) _param_fw_spoilers_lnd, + (ParamFloat) _param_fw_spoilers_desc, + (ParamInt) _param_fw_spoilers_man, + + (ParamFloat) _param_fw_man_p_max, + (ParamFloat) _param_fw_man_p_sc, + (ParamFloat) _param_fw_man_r_max, + (ParamFloat) _param_fw_man_r_sc, + (ParamFloat) _param_fw_man_y_sc, + + (ParamFloat) _param_fw_pr_ff, + (ParamFloat) _param_fw_pr_i, + (ParamFloat) _param_fw_pr_imax, + (ParamFloat) _param_fw_pr_p, + (ParamFloat) _param_fw_pr_d, + + (ParamFloat) _param_fw_rll_to_yaw_ff, + (ParamFloat) _param_fw_rr_ff, + (ParamFloat) _param_fw_rr_i, + (ParamFloat) _param_fw_rr_imax, + (ParamFloat) _param_fw_rr_p, + (ParamFloat) _param_fw_rr_d, + + (ParamFloat) _param_fw_yr_ff, + (ParamFloat) _param_fw_yr_i, + (ParamFloat) _param_fw_yr_imax, + (ParamFloat) _param_fw_yr_p, + (ParamFloat) _param_fw_yr_d, + + (ParamFloat) _param_trim_pitch, + (ParamFloat) _param_trim_roll, + (ParamFloat) _param_trim_yaw + ) + + RateControl _rate_control; ///< class for rate control calculations + + /** + * @brief Update flap control setting + * + * @param dt Current time delta [s] + */ + void controlFlaps(const float dt); + + /** + * @brief Update spoiler control setting + * + * @param dt Current time delta [s] + */ + void controlSpoilers(const float dt); + + void updateActuatorControlsStatus(float dt); + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + void vehicle_manual_poll(); + void vehicle_land_detected_poll(); + + float get_airspeed_and_update_scaling(); +}; diff --git a/src/modules/fw_rate_control/Kconfig b/src/modules/fw_rate_control/Kconfig new file mode 100644 index 0000000000..f22bc8e440 --- /dev/null +++ b/src/modules/fw_rate_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_FW_RATE_CONTROL + bool "fw_rate_control" + default n + ---help--- + Enable support for fw_att_control + +menuconfig USER_FW_RATE_CONTROL + bool "fw_rate_control running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_FW_RATE_CONTROL + ---help--- + Put fw_rate_control in userspace memory diff --git a/src/modules/fw_rate_control/fw_rate_control_params.c b/src/modules/fw_rate_control/fw_rate_control_params.c new file mode 100644 index 0000000000..e25590f436 --- /dev/null +++ b/src/modules/fw_rate_control/fw_rate_control_params.c @@ -0,0 +1,677 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file fw_rate_control_params.c + * + * Parameters defined by the fixed-wing attitude control task + * + * @author Lorenz Meier + * @author Thomas Gubler + */ + +/* + * Controller parameters, accessible via MAVLink + * + */ + +/** + * Minimum Airspeed (CAS) + * + * The minimal airspeed (calibrated airspeed) the user is able to command. + * Further, if the airspeed falls below this value, the TECS controller will try to + * increase airspeed more aggressively. + * Has to be set according to the vehicle's stall speed (which should be set in FW_AIRSPD_STALL), + * with some margin between the stall speed and minimum airspeed. + * This value corresponds to the desired minimum speed with the default load factor (level flight, default weight), + * and is automatically adpated to the current load factor (calculated from roll setpoint and WEIGHT_GROSS/WEIGHT_BASE). + * + * @unit m/s + * @min 0.5 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f); + +/** + * Maximum Airspeed (CAS) + * + * The maximal airspeed (calibrated airspeed) the user is able to command. + * Further, if the airspeed is above this value, the TECS controller will try to decrease + * airspeed more aggressively. + * + * @unit m/s + * @min 0.5 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f); + +/** + * Airspeed mode + * + * For small wings or VTOL without airspeed sensor this parameter can be used to + * enable flying without an airspeed reading + * + * @value 0 Normal (use airspeed if available) + * @value 1 Airspeed disabled + * @group FW Attitude Control + */ +PARAM_DEFINE_INT32(FW_ARSP_MODE, 0); + +/** + * Cruise Airspeed (CAS) + * + * The trim CAS (calibrated airspeed) of the vehicle. If an airspeed controller is active, + * this is the default airspeed setpoint that the controller will try to achieve if + * no other airspeed setpoint sources are present (e.g. through non-centered RC sticks). + * + * @unit m/s + * @min 0.5 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f); + +/** + * Stall Airspeed (CAS) + * + * The stall airspeed (calibrated airspeed) of the vehicle. + * It is used for airspeed sensor failure detection and for the control + * surface scaling airspeed limits. + * + * @unit m/s + * @min 0.5 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_STALL, 7.0f); + +/** + * Pitch rate proportional gain. + * + * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @unit %/rad/s + * @min 0.0 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f); + +/** + * Pitch rate derivative gain. + * + * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @unit %/rad/s + * @min 0.0 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_PR_D, 0.f); + +/** + * Pitch rate integrator gain. + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.0 + * @max 0.5 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_PR_I, 0.1f); + +/** + * Pitch rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); + +/** + * Roll rate proportional Gain + * + * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @unit %/rad/s + * @min 0.0 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); + +/** + * Roll rate derivative Gain + * + * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @unit %/rad/s + * @min 0.0 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_RR_D, 0.00f); + +/** + * Roll rate integrator Gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.0 + * @max 0.2 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_RR_I, 0.1f); + +/** + * Roll integrator anti-windup + * + * The portion of the integrator part in the control surface deflection is limited to this value. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f); + +/** + * Yaw rate proportional gain + * + * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @unit %/rad/s + * @min 0.0 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f); + +/** + * Yaw rate derivative gain + * + * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @unit %/rad/s + * @min 0.0 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_YR_D, 0.0f); + +/** + * Yaw rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.0 + * @max 50.0 + * @decimal 1 + * @increment 0.5 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_YR_I, 0.1f); + +/** + * Yaw rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); + +/** + * Roll rate feed forward + * + * Direct feed forward from rate setpoint to control surface output. Use this + * to obtain a tigher response of the controller without introducing + * noise amplification. + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f); + +/** + * Pitch rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); + +/** + * Yaw rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); + +/** + * Acro body x max rate. + * + * This is the rate the controller is trying to achieve if the user applies full roll + * stick input in acro mode. + * + * @min 45 + * @max 720 + * @unit deg + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90); + +/** + * Acro body y max rate. + * + * This is the body y rate the controller is trying to achieve if the user applies full pitch + * stick input in acro mode. + * + * @min 45 + * @max 720 + * @unit deg + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90); + +/** + * Acro body z max rate. + * + * This is the body z rate the controller is trying to achieve if the user applies full yaw + * stick input in acro mode. + * + * @min 10 + * @max 180 + * @unit deg + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45); + +/** + * Whether to scale throttle by battery power level + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. The fixed wing + * should constantly behave as if it was fully charged with reduced max thrust + * at lower battery percentages. i.e. if cruise speed is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group FW Rate Control + */ +PARAM_DEFINE_INT32(FW_BAT_SCALE_EN, 0); + +/** + * Enable airspeed scaling + * + * This enables a logic that automatically adjusts the output of the rate controller to take + * into account the real torque produced by an aerodynamic control surface given + * the current deviation from the trim airspeed (FW_AIRSPD_TRIM). + * + * Enable when using aerodynamic control surfaces (e.g.: plane) + * Disable when using rotor wings (e.g.: autogyro) + * + * @boolean + * @group FW Rate Control + */ +PARAM_DEFINE_INT32(FW_ARSP_SCALE_EN, 1); + +/** +* Roll trim increment at minimum airspeed +* +* This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MIN. + * + * @group FW Rate Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMIN, 0.0f); + +/** +* Pitch trim increment at minimum airspeed +* +* This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MIN. + * + * @group FW Rate Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMIN, 0.0f); + +/** +* Yaw trim increment at minimum airspeed +* +* This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MIN. + * + * @group FW Rate Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMIN, 0.0f); + +/** +* Roll trim increment at maximum airspeed +* +* This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MAX. + * + * @group FW Rate Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMAX, 0.0f); + +/** +* Pitch trim increment at maximum airspeed +* +* This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MAX. + * + * @group FW Rate Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMAX, 0.0f); + +/** +* Yaw trim increment at maximum airspeed +* +* This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MAX. + * + * @group FW Rate Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMAX, 0.0f); + +/** + * Roll trim increment for flaps configuration + * + * This increment is added to TRIM_ROLL whenever flaps are fully deployed. + * + * @group FW Rate Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_R_FLPS, 0.0f); + +/** + * Pitch trim increment for flaps configuration + * + * This increment is added to the pitch trim whenever flaps are fully deployed. + * + * @group FW Rate Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_P_FLPS, 0.0f); + +/** + * Pitch trim increment for spoiler configuration + * + * This increment is added to the pitch trim whenever spoilers are fully deployed. + * + * @group FW Rate Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_P_SPOIL, 0.f); + +/** + * Flaps setting during take-off + * + * Sets a fraction of full flaps during take-off. + * Also applies to flaperons if enabled in the mixer/allocation. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f); + +/** + * Flaps setting during landing + * + * Sets a fraction of full flaps during landing. + * Also applies to flaperons if enabled in the mixer/allocation. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f); + +/** + * Manual roll scale + * + * Scale factor applied to the desired roll actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_R_SC, 1.0f); + +/** + * Maximum manual pitch angle + * + * Maximum manual pitch angle setpoint (positive & negative) in manual attitude-only stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 30.0f); + +/** + * Manual pitch scale + * + * Scale factor applied to the desired pitch actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f); + +/** + * Maximum manual roll angle + * + * Maximum manual roll angle setpoint (positive & negative) in manual attitude-only stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); + +/** + * Manual yaw scale + * + * Scale factor applied to the desired yaw actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f); + +/** + * Roll control to yaw control feedforward gain. + * + * This gain can be used to counteract the "adverse yaw" effect for fixed wings. + * When the plane enters a roll it will tend to yaw the nose out of the turn. + * This gain enables the use of a yaw actuator (rudder, airbrakes, ...) to counteract + * this effect. + * + * @min 0.0 + * @decimal 1 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f); + +/** + * Spoiler landing setting + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f); + +/** + * Spoiler descend setting + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f); + +/** + * Spoiler input in manual flight + * + * Chose source for manual setting of spoilers in manual flight modes. + * + * @value 0 Disabled + * @value 1 Flaps channel + * @value 2 Aux1 + * @group FW Attitude Control + */ +PARAM_DEFINE_INT32(FW_SPOILERS_MAN, 0);