From 87c88cc9808498e6adf9eff8d025fce7c9027285 Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Fri, 14 Oct 2022 09:54:32 +0200 Subject: [PATCH] Fixedwing rate control into a separate module --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 1 + boards/airmind/mindpx-v2/default.px4board | 1 + boards/atl/mantis-edu/default.px4board | 1 + 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-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 | 383 +--------- .../FixedwingAttitudeControl.hpp | 93 +-- .../fw_att_control/fw_att_control_params.c | 487 ------------ src/modules/fw_rate_control/CMakeLists.txt | 44 ++ .../fw_rate_control/FixedwingRateControl.cpp | 690 ++++++++++++++++++ .../fw_rate_control/FixedwingRateControl.hpp | 252 +++++++ src/modules/fw_rate_control/Kconfig | 12 + .../fw_rate_control/fw_rate_control_params.c | 533 ++++++++++++++ 50 files changed, 1587 insertions(+), 950 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/boards/airmind/mindpx-v2/default.px4board b/boards/airmind/mindpx-v2/default.px4board index c3fae0e738..652f237c82 100644 --- a/boards/airmind/mindpx-v2/default.px4board +++ b/boards/airmind/mindpx-v2/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_GYRO_CALIBRATION=y diff --git a/boards/atl/mantis-edu/default.px4board b/boards/atl/mantis-edu/default.px4board index 88e1001ef0..2c0b4fcf4d 100644 --- a/boards/atl/mantis-edu/default.px4board +++ b/boards/atl/mantis-edu/default.px4board @@ -55,3 +55,4 @@ CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y diff --git a/boards/av/x-v1/default.px4board b/boards/av/x-v1/default.px4board index f1f793f5ac..f5be3c6310 100644 --- a/boards/av/x-v1/default.px4board +++ b/boards/av/x-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_GYRO_CALIBRATION=y diff --git a/boards/beaglebone/blue/default.px4board b/boards/beaglebone/blue/default.px4board index e28a13c13b..613bcc2449 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 0b2fee1e0f..f83cfd96a1 100644 --- a/boards/cuav/nora/default.px4board +++ b/boards/cuav/nora/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_GYRO_CALIBRATION=y diff --git a/boards/cuav/x7pro/default.px4board b/boards/cuav/x7pro/default.px4board index 48a94b2030..6463357aeb 100644 --- a/boards/cuav/x7pro/default.px4board +++ b/boards/cuav/x7pro/default.px4board @@ -52,6 +52,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_GYRO_CALIBRATION=y diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index fe85f68e6a..ed7dee2b67 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/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/cubepilot/cubeyellow/default.px4board b/boards/cubepilot/cubeyellow/default.px4board index de6c2abff0..182f43b20e 100644 --- a/boards/cubepilot/cubeyellow/default.px4board +++ b/boards/cubepilot/cubeyellow/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_GYRO_CALIBRATION=y diff --git a/boards/emlid/navio2/default.px4board b/boards/emlid/navio2/default.px4board index 48faec33f6..e48058f786 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 eaecd081bc..deff6df1d9 100644 --- a/boards/holybro/durandal-v1/default.px4board +++ b/boards/holybro/durandal-v1/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_GYRO_CALIBRATION=y diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 629c1a557f..4926ec8bda 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -39,6 +39,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 814050f587..4871745023 100644 --- a/boards/holybro/pix32v5/default.px4board +++ b/boards/holybro/pix32v5/default.px4board @@ -53,6 +53,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_GYRO_CALIBRATION=y diff --git a/boards/matek/h743-mini/default.px4board b/boards/matek/h743-mini/default.px4board index a9069bb741..fff8576c68 100644 --- a/boards/matek/h743-mini/default.px4board +++ b/boards/matek/h743-mini/default.px4board @@ -37,6 +37,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 bcff78646b..8945c33f42 100644 --- a/boards/matek/h743-slim/default.px4board +++ b/boards/matek/h743-slim/default.px4board @@ -36,6 +36,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 87395dd23d..80471641c8 100644 --- a/boards/matek/h743/default.px4board +++ b/boards/matek/h743/default.px4board @@ -37,6 +37,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 dbf4338931..34bc6ddb98 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 a49105cf96..8b3459d69f 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-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/mro/ctrl-zero-classic/default.px4board b/boards/mro/ctrl-zero-classic/default.px4board index d592c791f4..49a0c1738d 100644 --- a/boards/mro/ctrl-zero-classic/default.px4board +++ b/boards/mro/ctrl-zero-classic/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_GYRO_CALIBRATION=y diff --git a/boards/mro/ctrl-zero-f7-oem/default.px4board b/boards/mro/ctrl-zero-f7-oem/default.px4board index 1adec256f7..e217851893 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.px4board +++ b/boards/mro/ctrl-zero-f7-oem/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_GYRO_CALIBRATION=y diff --git a/boards/mro/ctrl-zero-f7/default.px4board b/boards/mro/ctrl-zero-f7/default.px4board index fca453d7d8..1ad81b7858 100644 --- a/boards/mro/ctrl-zero-f7/default.px4board +++ b/boards/mro/ctrl-zero-f7/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_GYRO_CALIBRATION=y diff --git a/boards/mro/ctrl-zero-h7-oem/default.px4board b/boards/mro/ctrl-zero-h7-oem/default.px4board index 5c64e1af3c..24c099ed57 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.px4board +++ b/boards/mro/ctrl-zero-h7-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-h7/default.px4board b/boards/mro/ctrl-zero-h7/default.px4board index 5fc4ad7dd8..e830c8ade6 100644 --- a/boards/mro/ctrl-zero-h7/default.px4board +++ b/boards/mro/ctrl-zero-h7/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/pixracerpro/default.px4board b/boards/mro/pixracerpro/default.px4board index f3c6137964..4de7a02c98 100644 --- a/boards/mro/pixracerpro/default.px4board +++ b/boards/mro/pixracerpro/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/x21-777/default.px4board b/boards/mro/x21-777/default.px4board index a7525e4e01..1e6c6dd322 100644 --- a/boards/mro/x21-777/default.px4board +++ b/boards/mro/x21-777/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_GYRO_CALIBRATION=y diff --git a/boards/mro/x21/default.px4board b/boards/mro/x21/default.px4board index bf9292ec98..73e9095297 100644 --- a/boards/mro/x21/default.px4board +++ b/boards/mro/x21/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_GYRO_CALIBRATION=y diff --git a/boards/nxp/fmuk66-e/default.px4board b/boards/nxp/fmuk66-e/default.px4board index 3772be3f2b..8188fe14ae 100644 --- a/boards/nxp/fmuk66-e/default.px4board +++ b/boards/nxp/fmuk66-e/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_GYRO_CALIBRATION=y diff --git a/boards/nxp/fmuk66-v3/default.px4board b/boards/nxp/fmuk66-v3/default.px4board index 63032fb003..7db5744559 100644 --- a/boards/nxp/fmuk66-v3/default.px4board +++ b/boards/nxp/fmuk66-v3/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-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 b1d9925cc8..e6165e1bb7 100644 --- a/boards/px4/fmu-v3/default.px4board +++ b/boards/px4/fmu-v3/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-v4/default.px4board b/boards/px4/fmu-v4/default.px4board index fe212199c0..cb6a7717e1 100644 --- a/boards/px4/fmu-v4/default.px4board +++ b/boards/px4/fmu-v4/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/px4/fmu-v4pro/default.px4board b/boards/px4/fmu-v4pro/default.px4board index f73c72d6bf..1d69e29ab3 100644 --- a/boards/px4/fmu-v4pro/default.px4board +++ b/boards/px4/fmu-v4pro/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_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index 0ac4351440..e83459f459 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-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index f0ff95bb6d..9c901439ba 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -58,6 +58,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 9f550cf3c0..0b57966adb 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 fc1ca57e40..d50e2b9935 100644 --- a/boards/px4/fmu-v6u/default.px4board +++ b/boards/px4/fmu-v6u/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-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 992238ede2..c16d7505f8 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 4533da476f..17923c6c9e 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 876a97632d..e4bd96991a 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 26b5e0cf8c..70ad243e82 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_GYRO_CALIBRATION=y diff --git a/boards/sky-drones/smartap-airlink/default.px4board b/boards/sky-drones/smartap-airlink/default.px4board index 943191213e..2b858aad16 100644 --- a/boards/sky-drones/smartap-airlink/default.px4board +++ b/boards/sky-drones/smartap-airlink/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/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 f967c44455..75e3d853ff 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,14 +37,11 @@ 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")) { @@ -56,10 +53,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : _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() @@ -94,20 +87,6 @@ FixedwingAttitudeControl::parameters_update() _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; } @@ -139,51 +118,26 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) // 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) { - if (_vcontrol_mode.flag_control_attitude_enabled) { - // STABILIZED mode generate the attitude setpoint from manual user inputs + // STABILIZED mode generate the attitude setpoint from manual user inputs - _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); + _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); - _att_sp.pitch_body = -_manual_control_setpoint.x * 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.x * 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] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + _att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw + _att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); - 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.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.y * radians(_param_fw_acro_x_max.get()); - _rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get()); - _rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get()); - _rates_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); - - _rate_sp_pub.publish(_rates_sp); - - } else { - /* manual/direct control */ - _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = - _manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get(); - _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = - -_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); - _actuator_controls.control[actuator_controls_s::INDEX_YAW] = - _manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f, - 1.0f); - } + _attitude_sp_pub.publish(_att_sp); } } } @@ -234,18 +188,6 @@ 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; } @@ -304,10 +246,6 @@ void FixedwingAttitudeControl::Run() float yawspeed = angular_velocity.xyz[2]; const Vector3f rates(rollspeed, pitchspeed, yawspeed); - vehicle_angular_acceleration_s angular_acceleration{}; - _vehicle_angular_acceleration_sub.copy(&angular_acceleration); - const Vector3f angular_accel{angular_acceleration.xyz}; - if (_vehicle_status.is_vtol_tailsitter) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * @@ -376,57 +314,23 @@ 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 integrals where needed */ - if (_att_sp.reset_rate_integrals) { - _rate_control.resetIntegral(); - } - /* Reset integrators if the aircraft is on ground * or a multicopter (but not transitioning VTOL or tailsitter) */ if (_landed || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) { - - _rate_control.resetIntegral(); _wheel_ctrl.reset_integrator(); } - // 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); - } - /* Prepare data for attitude controllers */ ECL_ControlData control_input{}; control_input.roll = euler_angles.phi(); @@ -483,38 +387,6 @@ void FixedwingAttitudeControl::Run() _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_stall.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_r_vmin.get(), - 0.0f); - trim_pitch += interpolate(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_p_vmin.get(), - 0.0f); - trim_yaw += interpolate(airspeed, _param_fw_airspd_stall.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(); - /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled) { if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { @@ -552,7 +424,6 @@ void FixedwingAttitudeControl::Run() /* add yaw rate setpoint from sticks in Stabilized mode */ if (_vcontrol_mode.flag_control_manual_enabled) { - _actuator_controls.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r; body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.r * radians(_param_fw_y_rmax.get()), -radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get())); } @@ -568,110 +439,10 @@ void FixedwingAttitudeControl::Run() } } - 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; - - float yaw_u = 0.0f; - - if (wheel_control) { - yaw_u = _wheel_ctrl.control_bodyrate(dt, control_input); - - // XXX: this is an abuse -- used to ferry manual yaw inputs from position controller during auto modes - yaw_u += _att_sp.yaw_sp_move_rate * _param_fw_man_y_sc.get(); - - } else { - const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2); - 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(); - - if (wheel_control) { - rate_ctrl_status.wheel_rate_integ = _wheel_ctrl.get_integrator(); - } - - _rate_ctrl_status_pub.publish(rate_ctrl_status); - } 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); - } - } - - updateActuatorControlsStatus(dt); } // backup schedule @@ -680,132 +451,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 4e65aad515..b0c70a5ca1 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 @@ -58,16 +54,12 @@ #include #include #include -#include -#include #include #include #include -#include #include #include #include -#include #include #include #include @@ -110,38 +102,26 @@ 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_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::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; - - 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)}; - actuator_controls_s _actuator_controls{}; manual_control_setpoint_s _manual_control_setpoint{}; vehicle_attitude_setpoint_s _att_sp{}; vehicle_control_mode_s _vcontrol_mode{}; @@ -155,21 +135,10 @@ 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; - DEFINE_PARAMETERS( (ParamFloat) _param_fw_acro_x_max, (ParamFloat) _param_fw_acro_y_max, @@ -180,50 +149,16 @@ private: (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, @@ -232,39 +167,13 @@ 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_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 - - /** - * @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. 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 5aa48e3af8..430fd00af6 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 1.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 0.5 - * @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 1.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 0.2 - * @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.2f); - /** * Maximum roll rate * @@ -239,63 +126,6 @@ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f); */ 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 1.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 50.0 - * @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.2f); - /** * Maximum yaw rate * @@ -311,21 +141,6 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); */ 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 * @@ -394,50 +209,6 @@ PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f); */ 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 10.0 - * @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 10.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 10.0 - * @decimal 2 - * @increment 0.05 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); - /** * Wheel steering rate feed forward * @@ -496,36 +267,6 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); */ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 30.0f); -/** - * 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 Attitude 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 Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f); - /** * Airspeed mode * @@ -538,234 +279,6 @@ PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f); */ 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 - * @unit deg - * @group FW Attitude 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 Attitude 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 Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45); - -/** -* 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 * 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..442419bc91 --- /dev/null +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -0,0 +1,690 @@ +/**************************************************************************** + * + * 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 (!_att_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_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 +FixedwingRateControl::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)) { + + // 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_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.y * radians(_param_fw_acro_x_max.get()); + _rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get()); + _rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get()); + _rates_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + + _rate_sp_pub.publish(_rates_sp); + + } else { + /* manual/direct control */ + _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = + _manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get(); + _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = + -_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); + _actuator_controls.control[actuator_controls_s::INDEX_YAW] = + _manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); + _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f, + 1.0f); + } + } + } + } +} + +void +FixedwingRateControl::vehicle_attitude_setpoint_poll() +{ + _att_sp_sub.update(&_att_sp); +} + +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()) { + _att_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // only run controller if attitude changed + if (_att_sub.updated() || (hrt_elapsed_time(&_last_run) > 20_ms)) { + + // 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_attitude_s att{}; + + if (_att_sub.copy(&att)) { + dt = math::constrain((att.timestamp_sample - _last_run) * 1e-6f, DT_MIN, DT_MAX); + _last_run = att.timestamp_sample; + + // get current rotation matrix and euler angles from control state quaternions + _R = matrix::Quatf(att.q); + } + + 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_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); + + vehicle_angular_acceleration_s angular_acceleration{}; + _vehicle_angular_acceleration_sub.copy(&angular_acceleration); + const Vector3f angular_accel{angular_acceleration.xyz}; + + if (_vehicle_status.is_vtol_tailsitter) { + /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode + * + * Since the VTOL airframe is initialized as a multicopter we need to + * modify the estimated attitude for the fixed wing operation. + * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around + * the pitch axis compared to the neutral position of the vehicle in multicopter mode + * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. + * Additionally, in order to get the correct sign of the pitch, we need to multiply + * the new x axis of the rotation matrix with -1 + * + * original: modified: + * + * Rxx Ryx Rzx -Rzx Ryx Rxx + * Rxy Ryy Rzy -Rzy Ryy Rxy + * Rxz Ryz Rzz -Rzz Ryz Rxz + * */ + matrix::Dcmf R_adapted = _R; //modified rotation matrix + + /* move z to x */ + R_adapted(0, 0) = _R(0, 2); + R_adapted(1, 0) = _R(1, 2); + R_adapted(2, 0) = _R(2, 2); + + /* move x to z */ + R_adapted(0, 2) = _R(0, 0); + R_adapted(1, 2) = _R(1, 0); + R_adapted(2, 2) = _R(2, 0); + + /* change direction of pitch (convert to right handed system) */ + R_adapted(0, 0) = -R_adapted(0, 0); + R_adapted(1, 0) = -R_adapted(1, 0); + R_adapted(2, 0) = -R_adapted(2, 0); + + /* fill in new attitude data */ + _R = R_adapted; + + /* lastly, roll- and yawspeed have to be swaped */ + float helper = rollspeed; + rollspeed = -yawspeed; + yawspeed = helper; + } + + const matrix::Eulerf euler_angles(_R); + + 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_sub.update(&_vehicle_status); + + vehicle_control_mode_poll(); + vehicle_manual_poll(euler_angles.psi()); + 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 (_att_sp.reset_rate_integrals) { + _rate_control.resetIntegral(); + } + + /* Reset integrators if the aircraft is on ground + * or a multicopter (but not transitioning VTOL or tailsitter) + */ + if (_landed + || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) { + + _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); + } + + _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_stall.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_r_vmin.get(), + 0.0f); + trim_pitch += interpolate(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_p_vmin.get(), + 0.0f); + trim_yaw += interpolate(airspeed, _param_fw_airspd_stall.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 = 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); + } + } + + 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 attitude 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..ba65a85b2b --- /dev/null +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -0,0 +1,252 @@ +/**************************************************************************** + * + * 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 +#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 _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 _battery_status_sub{ORB_ID(battery_status)}; /**< battery status 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_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::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; + + 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{}; + + matrix::Dcmf _R{matrix::eye()}; + + perf_counter_t _loop_perf; + + 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; + + 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_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_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(); +}; 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..5f6848cbbb --- /dev/null +++ b/src/modules/fw_rate_control/fw_rate_control_params.c @@ -0,0 +1,533 @@ +/**************************************************************************** + * + * 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 + * + */ + +/** + * 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); + +/** + * 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); + +/** + * 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);