diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 81af5e1acd7a..7597c989b3c5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -15,7 +15,8 @@ control_allocator start # fw_rate_control start fw_att_control start -fw_pos_control start +fw_mode_manager start +fw_lat_lon_control start airspeed_selector start # diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index c28adef56ae9..16fb6ee5d660 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -27,7 +27,8 @@ fi fw_rate_control start vtol fw_att_control start vtol -fw_pos_control start vtol +fw_mode_manager start +fw_lat_lon_control start vtol fw_autotune_attitude_control start vtol # Start Land Detector diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board b/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board index 549ba99b7331..326bb98a9390 100644 --- a/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board +++ b/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board @@ -41,7 +41,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/airmind/mindpx-v2/default.px4board b/boards/airmind/mindpx-v2/default.px4board index 33ad75b6957b..67c4a57dd4e2 100644 --- a/boards/airmind/mindpx-v2/default.px4board +++ b/boards/airmind/mindpx-v2/default.px4board @@ -46,7 +46,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/ark/fmu-v6x/default.px4board b/boards/ark/fmu-v6x/default.px4board index 0ec7b49c4252..64b2ff983873 100644 --- a/boards/ark/fmu-v6x/default.px4board +++ b/boards/ark/fmu-v6x/default.px4board @@ -56,7 +56,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/ark/fpv/default.px4board b/boards/ark/fpv/default.px4board index f1f021b44e8c..79f3ab19dac2 100644 --- a/boards/ark/fpv/default.px4board +++ b/boards/ark/fpv/default.px4board @@ -37,7 +37,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/auterion/fmu-v6s/default.px4board b/boards/auterion/fmu-v6s/default.px4board index bb1bd7fdc0bd..d99a2ae6a1ed 100644 --- a/boards/auterion/fmu-v6s/default.px4board +++ b/boards/auterion/fmu-v6s/default.px4board @@ -37,7 +37,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_FIGURE_OF_EIGHT=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/auterion/fmu-v6s/multicopter.px4board b/boards/auterion/fmu-v6s/multicopter.px4board index 296a35c21fc9..69617bda0ca0 100644 --- a/boards/auterion/fmu-v6s/multicopter.px4board +++ b/boards/auterion/fmu-v6s/multicopter.px4board @@ -3,7 +3,8 @@ CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=n CONFIG_MODULES_AIRSPEED_SELECTOR=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_COMMON_RC=y diff --git a/boards/auterion/fmu-v6s/rover.px4board b/boards/auterion/fmu-v6s/rover.px4board index f754c54e36f7..2709b271b6c8 100644 --- a/boards/auterion/fmu-v6s/rover.px4board +++ b/boards/auterion/fmu-v6s/rover.px4board @@ -2,7 +2,8 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=n CONFIG_MODULES_FLIGHT_MODE_MANAGER=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n CONFIG_MODULES_MC_ATT_CONTROL=n diff --git a/boards/av/x-v1/default.px4board b/boards/av/x-v1/default.px4board index 86db7c85a3d5..1ad68a8f7117 100644 --- a/boards/av/x-v1/default.px4board +++ b/boards/av/x-v1/default.px4board @@ -42,7 +42,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/beaglebone/blue/default.px4board b/boards/beaglebone/blue/default.px4board index 79e0d80b9276..1e7db4fae557 100644 --- a/boards/beaglebone/blue/default.px4board +++ b/boards/beaglebone/blue/default.px4board @@ -30,7 +30,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/bluerobotics/navigator/default.px4board b/boards/bluerobotics/navigator/default.px4board index 49ff0a2abad2..8812f0036620 100644 --- a/boards/bluerobotics/navigator/default.px4board +++ b/boards/bluerobotics/navigator/default.px4board @@ -49,7 +49,8 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MC_ATT_CONTROL=y diff --git a/boards/corvon/743v1/default.px4board b/boards/corvon/743v1/default.px4board index 149bcaff215c..ac84a53cd60b 100644 --- a/boards/corvon/743v1/default.px4board +++ b/boards/corvon/743v1/default.px4board @@ -38,7 +38,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/cuav/7-nano/default.px4board b/boards/cuav/7-nano/default.px4board index f6e4511ae2da..eb1dc29bc377 100644 --- a/boards/cuav/7-nano/default.px4board +++ b/boards/cuav/7-nano/default.px4board @@ -42,7 +42,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/cuav/nora/default.px4board b/boards/cuav/nora/default.px4board index 675ffa54cb3e..b0db9a173587 100644 --- a/boards/cuav/nora/default.px4board +++ b/boards/cuav/nora/default.px4board @@ -50,7 +50,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/cuav/x7pro/default.px4board b/boards/cuav/x7pro/default.px4board index c7c30b1a1ed2..eafa9643720f 100644 --- a/boards/cuav/x7pro/default.px4board +++ b/boards/cuav/x7pro/default.px4board @@ -50,7 +50,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index 9bb4f368f944..f10622bb3f51 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -49,7 +49,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/cubepilot/cubeorangeplus/default.px4board b/boards/cubepilot/cubeorangeplus/default.px4board index ba6ed0b34f21..8b2c0839b908 100644 --- a/boards/cubepilot/cubeorangeplus/default.px4board +++ b/boards/cubepilot/cubeorangeplus/default.px4board @@ -50,7 +50,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/cubepilot/cubeyellow/default.px4board b/boards/cubepilot/cubeyellow/default.px4board index 62cf898af96c..8d0dc3eee3e0 100644 --- a/boards/cubepilot/cubeyellow/default.px4board +++ b/boards/cubepilot/cubeyellow/default.px4board @@ -46,7 +46,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/emlid/navio2/default.px4board b/boards/emlid/navio2/default.px4board index 722f42b410d0..6498bf7820f0 100644 --- a/boards/emlid/navio2/default.px4board +++ b/boards/emlid/navio2/default.px4board @@ -32,7 +32,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/hkust/nxt-dual/default.px4board b/boards/hkust/nxt-dual/default.px4board index 4327646414a0..97b19358e551 100644 --- a/boards/hkust/nxt-dual/default.px4board +++ b/boards/hkust/nxt-dual/default.px4board @@ -39,7 +39,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/hkust/nxt-v1/default.px4board b/boards/hkust/nxt-v1/default.px4board index b72160775850..0690280eea6a 100644 --- a/boards/hkust/nxt-v1/default.px4board +++ b/boards/hkust/nxt-v1/default.px4board @@ -41,7 +41,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/holybro/durandal-v1/default.px4board b/boards/holybro/durandal-v1/default.px4board index ecc1bc81013b..627917d186ee 100644 --- a/boards/holybro/durandal-v1/default.px4board +++ b/boards/holybro/durandal-v1/default.px4board @@ -44,7 +44,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/holybro/kakuteh7-wing/default.px4board b/boards/holybro/kakuteh7-wing/default.px4board index f8440af002cb..08670e4781fa 100644 --- a/boards/holybro/kakuteh7-wing/default.px4board +++ b/boards/holybro/kakuteh7-wing/default.px4board @@ -37,7 +37,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index f4a8e0c82407..fed11068e690 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -45,7 +45,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/holybro/kakuteh7dualimu/default.px4board b/boards/holybro/kakuteh7dualimu/default.px4board index fbecd9f5af8f..5b4f4710a4e4 100644 --- a/boards/holybro/kakuteh7dualimu/default.px4board +++ b/boards/holybro/kakuteh7dualimu/default.px4board @@ -48,7 +48,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board index c023eae04fe5..06bc5ffe7ed1 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -45,7 +45,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/holybro/kakuteh7v2/default.px4board b/boards/holybro/kakuteh7v2/default.px4board index 0f1b80b25095..8821361a6938 100644 --- a/boards/holybro/kakuteh7v2/default.px4board +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -45,7 +45,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/holybro/pix32v5/default.px4board b/boards/holybro/pix32v5/default.px4board index 0093ee2dcfd0..01c28a922c4a 100644 --- a/boards/holybro/pix32v5/default.px4board +++ b/boards/holybro/pix32v5/default.px4board @@ -50,7 +50,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/matek/h743-mini/default.px4board b/boards/matek/h743-mini/default.px4board index 22efafcfdb38..965fec54592e 100644 --- a/boards/matek/h743-mini/default.px4board +++ b/boards/matek/h743-mini/default.px4board @@ -30,7 +30,8 @@ CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=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 48e937fc43c1..9cf0909725ed 100644 --- a/boards/matek/h743-slim/default.px4board +++ b/boards/matek/h743-slim/default.px4board @@ -33,7 +33,8 @@ CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=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 71024fea5ceb..e61c9b6014e9 100644 --- a/boards/matek/h743/default.px4board +++ b/boards/matek/h743/default.px4board @@ -32,7 +32,8 @@ CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/micoair/h743-aio/default.px4board b/boards/micoair/h743-aio/default.px4board index 2d92e89d967f..5e68a6dbc5b3 100644 --- a/boards/micoair/h743-aio/default.px4board +++ b/boards/micoair/h743-aio/default.px4board @@ -39,7 +39,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/micoair/h743-v2/default.px4board b/boards/micoair/h743-v2/default.px4board index 74f1e2de2979..821021a7535b 100644 --- a/boards/micoair/h743-v2/default.px4board +++ b/boards/micoair/h743-v2/default.px4board @@ -40,7 +40,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/micoair/h743/default.px4board b/boards/micoair/h743/default.px4board index 149bcaff215c..ac84a53cd60b 100644 --- a/boards/micoair/h743/default.px4board +++ b/boards/micoair/h743/default.px4board @@ -38,7 +38,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=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 b73fdd18f873..12623c2c87f7 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -49,7 +49,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board index f95c859e8c52..38a9641d337f 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -40,7 +40,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/mro/ctrl-zero-classic/default.px4board b/boards/mro/ctrl-zero-classic/default.px4board index 665fbc8626f4..16cac64d3fae 100644 --- a/boards/mro/ctrl-zero-classic/default.px4board +++ b/boards/mro/ctrl-zero-classic/default.px4board @@ -46,7 +46,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=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 9fe340b87f1b..5d35d0b04f8f 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.px4board +++ b/boards/mro/ctrl-zero-f7-oem/default.px4board @@ -44,7 +44,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=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 d09ca8a5b50f..8e33ae6bb925 100644 --- a/boards/mro/ctrl-zero-f7/default.px4board +++ b/boards/mro/ctrl-zero-f7/default.px4board @@ -44,7 +44,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=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 ec6c59888db5..bbb20fd25222 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.px4board +++ b/boards/mro/ctrl-zero-h7-oem/default.px4board @@ -42,7 +42,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/mro/ctrl-zero-h7/default.px4board b/boards/mro/ctrl-zero-h7/default.px4board index 7bb96d3aca08..d858efba2118 100644 --- a/boards/mro/ctrl-zero-h7/default.px4board +++ b/boards/mro/ctrl-zero-h7/default.px4board @@ -43,7 +43,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/mro/pixracerpro/default.px4board b/boards/mro/pixracerpro/default.px4board index fb49c90866cb..77c80f8de4fc 100644 --- a/boards/mro/pixracerpro/default.px4board +++ b/boards/mro/pixracerpro/default.px4board @@ -43,7 +43,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/mro/x21-777/default.px4board b/boards/mro/x21-777/default.px4board index cec5dd074a85..8512dd375ec5 100644 --- a/boards/mro/x21-777/default.px4board +++ b/boards/mro/x21-777/default.px4board @@ -45,7 +45,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/mro/x21/default.px4board b/boards/mro/x21/default.px4board index 8640bff623b0..749c36e211ce 100644 --- a/boards/mro/x21/default.px4board +++ b/boards/mro/x21/default.px4board @@ -46,7 +46,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/nxp/fmuk66-e/default.px4board b/boards/nxp/fmuk66-e/default.px4board index 749e62e5d54f..70f4c16968d7 100644 --- a/boards/nxp/fmuk66-e/default.px4board +++ b/boards/nxp/fmuk66-e/default.px4board @@ -47,7 +47,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/nxp/fmuk66-v3/default.px4board b/boards/nxp/fmuk66-v3/default.px4board index 268e89c70fd8..a8edef6f01ce 100644 --- a/boards/nxp/fmuk66-v3/default.px4board +++ b/boards/nxp/fmuk66-v3/default.px4board @@ -48,7 +48,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/nxp/mr-canhubk3/fmu.px4board b/boards/nxp/mr-canhubk3/fmu.px4board index 48a7883fe259..a1a3d9ddd1aa 100644 --- a/boards/nxp/mr-canhubk3/fmu.px4board +++ b/boards/nxp/mr-canhubk3/fmu.px4board @@ -33,7 +33,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/nxp/mr-canhubk3/sysview.px4board b/boards/nxp/mr-canhubk3/sysview.px4board index 2089b09931c5..a101423cf98f 100644 --- a/boards/nxp/mr-canhubk3/sysview.px4board +++ b/boards/nxp/mr-canhubk3/sysview.px4board @@ -20,7 +20,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/nxp/mr-canhubk3/zenoh.px4board b/boards/nxp/mr-canhubk3/zenoh.px4board index fe2297a5547f..9bc71357c455 100644 --- a/boards/nxp/mr-canhubk3/zenoh.px4board +++ b/boards/nxp/mr-canhubk3/zenoh.px4board @@ -24,7 +24,8 @@ CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/nxp/tropic-community/default.px4board b/boards/nxp/tropic-community/default.px4board index c7fcb205e02f..93e80bba1114 100644 --- a/boards/nxp/tropic-community/default.px4board +++ b/boards/nxp/tropic-community/default.px4board @@ -47,7 +47,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v2/fixedwing.px4board b/boards/px4/fmu-v2/fixedwing.px4board index 0bde0c7e0768..b6fd9c743b57 100644 --- a/boards/px4/fmu-v2/fixedwing.px4board +++ b/boards/px4/fmu-v2/fixedwing.px4board @@ -8,5 +8,6 @@ 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_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y diff --git a/boards/px4/fmu-v3/default.px4board b/boards/px4/fmu-v3/default.px4board index 5dec8fda22ed..386b363aa6a8 100644 --- a/boards/px4/fmu-v3/default.px4board +++ b/boards/px4/fmu-v3/default.px4board @@ -51,7 +51,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v4/default.px4board b/boards/px4/fmu-v4/default.px4board index e7906db52c85..48ecf1b2c2b9 100644 --- a/boards/px4/fmu-v4/default.px4board +++ b/boards/px4/fmu-v4/default.px4board @@ -52,7 +52,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v4pro/default.px4board b/boards/px4/fmu-v4pro/default.px4board index ff8455c28588..a30e386ea989 100644 --- a/boards/px4/fmu-v4pro/default.px4board +++ b/boards/px4/fmu-v4pro/default.px4board @@ -47,7 +47,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index 1132a48bbfb4..e5da7855a792 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -55,7 +55,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v5/protected.px4board b/boards/px4/fmu-v5/protected.px4board index 7ea307124c37..9f8eb3288813 100644 --- a/boards/px4/fmu-v5/protected.px4board +++ b/boards/px4/fmu-v5/protected.px4board @@ -17,7 +17,8 @@ CONFIG_MODULES_CAMERA_FEEDBACK=n CONFIG_MODULES_ESC_BATTERY=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=n diff --git a/boards/px4/fmu-v5/rover.px4board b/boards/px4/fmu-v5/rover.px4board index 2c9e2d20d3d5..1d4d56e9a75f 100644 --- a/boards/px4/fmu-v5/rover.px4board +++ b/boards/px4/fmu-v5/rover.px4board @@ -4,7 +4,8 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n CONFIG_MODULES_FLIGHT_MODE_MANAGER=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n CONFIG_MODULES_MC_ATT_CONTROL=n diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board index e8bcadebd71e..8e96a2ff79a7 100644 --- a/boards/px4/fmu-v5/stackcheck.px4board +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -29,7 +29,8 @@ CONFIG_MODULES_ESC_BATTERY=n CONFIG_MODULES_EVENTS=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_GIMBAL=n CONFIG_MODULES_GYRO_CALIBRATION=n diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 8ba41726c086..77a92afc7fcb 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -59,7 +59,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v5x/rover.px4board b/boards/px4/fmu-v5x/rover.px4board index 42f8ec21ce5d..916b00989753 100644 --- a/boards/px4/fmu-v5x/rover.px4board +++ b/boards/px4/fmu-v5x/rover.px4board @@ -3,7 +3,8 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=n CONFIG_MODULES_FLIGHT_MODE_MANAGER=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n CONFIG_MODULES_MC_ATT_CONTROL=n diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index e6b812fffd8f..e3aa477534e4 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -47,7 +47,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v6c/rover.px4board b/boards/px4/fmu-v6c/rover.px4board index 9eed13c66530..ee2e5a13e516 100644 --- a/boards/px4/fmu-v6c/rover.px4board +++ b/boards/px4/fmu-v6c/rover.px4board @@ -2,7 +2,8 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=n CONFIG_MODULES_FLIGHT_MODE_MANAGER=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n CONFIG_MODULES_MC_ATT_CONTROL=n diff --git a/boards/px4/fmu-v6u/default.px4board b/boards/px4/fmu-v6u/default.px4board index be90aca727bb..e72a584fd8cb 100644 --- a/boards/px4/fmu-v6u/default.px4board +++ b/boards/px4/fmu-v6u/default.px4board @@ -47,7 +47,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v6u/rover.px4board b/boards/px4/fmu-v6u/rover.px4board index 9eed13c66530..ee2e5a13e516 100644 --- a/boards/px4/fmu-v6u/rover.px4board +++ b/boards/px4/fmu-v6u/rover.px4board @@ -2,7 +2,8 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=n CONFIG_MODULES_FLIGHT_MODE_MANAGER=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n CONFIG_MODULES_MC_ATT_CONTROL=n diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index ba42e02a9d94..778eb50198ae 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -57,7 +57,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v6x/multicopter.px4board b/boards/px4/fmu-v6x/multicopter.px4board index 9f540f567d09..147297d55e79 100644 --- a/boards/px4/fmu-v6x/multicopter.px4board +++ b/boards/px4/fmu-v6x/multicopter.px4board @@ -3,7 +3,8 @@ CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=n CONFIG_MODULES_AIRSPEED_SELECTOR=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_COMMON_RC=y diff --git a/boards/px4/fmu-v6x/rover.px4board b/boards/px4/fmu-v6x/rover.px4board index 9eed13c66530..ee2e5a13e516 100644 --- a/boards/px4/fmu-v6x/rover.px4board +++ b/boards/px4/fmu-v6x/rover.px4board @@ -2,7 +2,8 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=n CONFIG_MODULES_FLIGHT_MODE_MANAGER=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n CONFIG_MODULES_MC_ATT_CONTROL=n diff --git a/boards/px4/fmu-v6xrt/default.px4board b/boards/px4/fmu-v6xrt/default.px4board index 477ba76ed8f5..b990cc48aebd 100644 --- a/boards/px4/fmu-v6xrt/default.px4board +++ b/boards/px4/fmu-v6xrt/default.px4board @@ -57,7 +57,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v6xrt/rover.px4board b/boards/px4/fmu-v6xrt/rover.px4board index 7b8ed3bd1f5a..458ebe7d0802 100644 --- a/boards/px4/fmu-v6xrt/rover.px4board +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -3,7 +3,8 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=n CONFIG_MODULES_FLIGHT_MODE_MANAGER=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n diff --git a/boards/px4/raspberrypi/default.px4board b/boards/px4/raspberrypi/default.px4board index ade7dd42358d..484a66beff9a 100644 --- a/boards/px4/raspberrypi/default.px4board +++ b/boards/px4/raspberrypi/default.px4board @@ -30,7 +30,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index f85c03e23a2a..28c16f931fc0 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -19,7 +19,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_FIGURE_OF_EIGHT=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y diff --git a/boards/px4/sitl/spacecraft.px4board b/boards/px4/sitl/spacecraft.px4board index 56c966598bcb..44ac04d7ddae 100644 --- a/boards/px4/sitl/spacecraft.px4board +++ b/boards/px4/sitl/spacecraft.px4board @@ -2,7 +2,8 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=n CONFIG_MODULES_FLIGHT_MODE_MANAGER=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_MC_ATT_CONTROL=n diff --git a/boards/scumaker/pilotpi/default.px4board b/boards/scumaker/pilotpi/default.px4board index b72da494da23..d9d35ab25dbc 100644 --- a/boards/scumaker/pilotpi/default.px4board +++ b/boards/scumaker/pilotpi/default.px4board @@ -32,7 +32,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/siyi/n7/default.px4board b/boards/siyi/n7/default.px4board index 2a7a23258186..3dafcc85a148 100644 --- a/boards/siyi/n7/default.px4board +++ b/boards/siyi/n7/default.px4board @@ -38,7 +38,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=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 fdfdcdea6a1b..b2cd40a7ba90 100644 --- a/boards/sky-drones/smartap-airlink/default.px4board +++ b/boards/sky-drones/smartap-airlink/default.px4board @@ -46,7 +46,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/thepeach/k1/default.px4board b/boards/thepeach/k1/default.px4board index d760279546c7..cc519b968dfd 100644 --- a/boards/thepeach/k1/default.px4board +++ b/boards/thepeach/k1/default.px4board @@ -44,7 +44,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/thepeach/r1/default.px4board b/boards/thepeach/r1/default.px4board index d760279546c7..cc519b968dfd 100644 --- a/boards/thepeach/r1/default.px4board +++ b/boards/thepeach/r1/default.px4board @@ -44,7 +44,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/x-mav/ap-h743v2/default.px4board b/boards/x-mav/ap-h743v2/default.px4board index 09a32b78c512..33f81262d3c5 100644 --- a/boards/x-mav/ap-h743v2/default.px4board +++ b/boards/x-mav/ap-h743v2/default.px4board @@ -40,7 +40,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/zeroone/x6/default.px4board b/boards/zeroone/x6/default.px4board index ba05f4ab5fe4..20677f9d1f39 100644 --- a/boards/zeroone/x6/default.px4board +++ b/boards/zeroone/x6/default.px4board @@ -47,7 +47,8 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=y +CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/docs/en/flight_modes_fw/land.md b/docs/en/flight_modes_fw/land.md index 3c294e95f033..43d8ff92942c 100644 --- a/docs/en/flight_modes_fw/land.md +++ b/docs/en/flight_modes_fw/land.md @@ -45,6 +45,7 @@ Land mode behaviour can be configured using the parameters below. | [NAV_LOITER_RAD](../advanced_config/parameter_reference.md#NAV_LOITER_RAD) | The loiter radius that the controller tracks for the whole landing sequence. | | [FW_LND_ANG](../advanced_config/parameter_reference.md#FW_LND_ANG) | The flight path angle setpoint. | | [FW_LND_AIRSPD](../advanced_config/parameter_reference.md#FW_LND_AIRSPD) | The airspeed setpoint. | +| [FW_AIRSPD_FLP_SC](../advanced_config/parameter_reference.md#FW_AIRSPD_FLP_SC) | Factor applied to the minimum airspeed when flaps are fully deployed. Necessary if FW_LND_AIRSPD is below FW_AIRSPD_MIN. ## See Also diff --git a/docs/en/flight_modes_fw/takeoff.md b/docs/en/flight_modes_fw/takeoff.md index ea82c74d60fd..a59413394dcc 100644 --- a/docs/en/flight_modes_fw/takeoff.md +++ b/docs/en/flight_modes_fw/takeoff.md @@ -50,6 +50,7 @@ Parameters that affect both catapult/hand-launch and runway takeoffs: | [FW_TKO_PITCH_MIN](../advanced_config/parameter_reference.md#FW_TKO_PITCH_MIN) | This is the minimum pitch angle setpoint during the climbout phase | | [FW_T_CLMB_MAX](../advanced_config/parameter_reference.md#FW_T_CLMB_MAX) | Maximum climb rate. | | [FW_FLAPS_TO_SCL](../advanced_config/parameter_reference.md#FW_FLAPS_TO_SCL) | Flaps setpoint during takeoff | +| [FW_AIRSPD_FLP_SC](../advanced_config/parameter_reference.md#FW_AIRSPD_FLP_SC) | Factor applied to the minimum airspeed when flaps are fully deployed. Necessary if FW_TKO_AIRSPD is below FW_AIRSPD_MIN. | ::: info The vehicle always respects normal FW max/min throttle settings during takeoff ([FW_THR_MIN](../advanced_config/parameter_reference.md#FW_THR_MIN), [FW_THR_MAX](../advanced_config/parameter_reference.md#FW_THR_MAX)). @@ -101,7 +102,7 @@ The _runway takeoff mode_ has the following phases: ::: info For a smooth takeoff, the runway wheel controller possibly needs to be tuned. -It consists of a rate controller (P-I-FF-controller with the parameters [FW_WR_P](../advanced_config/parameter_reference.md#FW_WR_P), [FW_WR_I](../advanced_config/parameter_reference.md#FW_WR_I), [FW_WR_FF](../advanced_config/parameter_reference.md#FW_WR_FF)) and an outer loop that calculates heading setpoints from course errors and can be tuned via [RWTO_NPFG_PERIOD](#RWTO_NPFG_PERIOD). +It consists of a rate controller (P-I-FF-controller with the parameters [FW_WR_P](../advanced_config/parameter_reference.md#FW_WR_P), [FW_WR_I](../advanced_config/parameter_reference.md#FW_WR_I), [FW_WR_FF](../advanced_config/parameter_reference.md#FW_WR_FF)). ::: ### Parameters (Runway Takeoff) @@ -120,7 +121,6 @@ Runway takeoff is affected by the following parameters: | [RWTO_NUDGE](../advanced_config/parameter_reference.md#RWTO_NUDGE) | Enable wheel controller nudging while on the runway | | [FW_WING_SPAN](../advanced_config/parameter_reference.md#FW_WING_SPAN) | The wingspan of the vehicle. Used to prevent wingstrikes. | | [FW_WING_HEIGHT](../advanced_config/parameter_reference.md#FW_WING_HEIGHT) | The height of the wings above ground (ground clearance). Used to prevent wingstrikes. | -| [RWTO_NPFG_PERIOD](../advanced_config/parameter_reference.md#RWTO_NPFG_PERIOD) | L1 period while steering on runway. Increase for less aggressive response to course errors. | ## See Also diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 6c374f2c35b8..a0cc5ab1ec8b 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -90,6 +90,9 @@ set(msg_files FollowTargetEstimator.msg FollowTargetStatus.msg FuelTankStatus.msg + FixedWingLateralGuidanceStatus.msg + FixedWingLateralStatus.msg + FixedWingRunwayControl.msg GeneratorStatus.msg GeofenceResult.msg GeofenceStatus.msg @@ -137,7 +140,6 @@ set(msg_files NavigatorMissionItem.msg NavigatorStatus.msg NormalizedUnsignedSetpoint.msg - NpfgStatus.msg ObstacleDistance.msg OffboardControlMode.msg OnboardComputerStatus.msg @@ -241,8 +243,12 @@ set(msg_files versioned/ArmingCheckRequest.msg versioned/BatteryStatus.msg versioned/ConfigOverrides.msg + versioned/FixedWingLateralSetpoint.msg + versioned/FixedWingLongitudinalSetpoint.msg versioned/GotoSetpoint.msg versioned/HomePosition.msg + versioned/LateralControlConfiguration.msg + versioned/LongitudinalControlConfiguration.msg versioned/ManualControlSetpoint.msg versioned/ModeCompleted.msg versioned/RegisterExtComponentReply.msg diff --git a/msg/FixedWingLateralGuidanceStatus.msg b/msg/FixedWingLateralGuidanceStatus.msg new file mode 100644 index 000000000000..57a9df513631 --- /dev/null +++ b/msg/FixedWingLateralGuidanceStatus.msg @@ -0,0 +1,13 @@ +# Fixed Wing Lateral Guidance Status message +# Published by fw_pos_control module to report the resultant lateral setpoints and NPFG debug outputs + +uint64 timestamp # time since system start (microseconds) + +float32 course_setpoint # [rad] [@range -pi, pi] Desired direction of travel over ground w.r.t (true) North. Set by guidance law +float32 lateral_acceleration_ff # [m/s^2] [FRD] lateral acceleration demand only for maintaining curvature +float32 bearing_feas # [@range 0,1] bearing feasibility +float32 bearing_feas_on_track # [@range 0,1] on-track bearing feasibility +float32 signed_track_error # [m] signed track error +float32 track_error_bound # [m] track error bound +float32 adapted_period # [s] adapted period (if auto-tuning enabled) +uint8 wind_est_valid # [boolean] true = wind estimate is valid and/or being used by controller (also indicates if wind estimate usage is disabled despite being valid) diff --git a/msg/FixedWingLateralStatus.msg b/msg/FixedWingLateralStatus.msg new file mode 100644 index 000000000000..484393706982 --- /dev/null +++ b/msg/FixedWingLateralStatus.msg @@ -0,0 +1,7 @@ +# Fixed Wing Lateral Status message +# Published by the fw_lateral_longitudinal_control module to report the resultant lateral setpoint + +uint64 timestamp # time since system start (microseconds) + +float32 lateral_acceleration_setpoint # [m/s^2] [FRD] resultant lateral acceleration setpoint +float32 can_run_factor # [norm] [@range 0, 1] estimate of certainty of the correct functionality of the npfg roll setpoint diff --git a/msg/FixedWingRunwayControl.msg b/msg/FixedWingRunwayControl.msg new file mode 100644 index 000000000000..62d09a5ebf65 --- /dev/null +++ b/msg/FixedWingRunwayControl.msg @@ -0,0 +1,8 @@ +# Auxiliary control fields for fixed-wing runway takeoff/landing + +# Passes information from the FixedWingModeManager to the FixedWingAttitudeController + +uint64 timestamp # [us] time since system start + +bool wheel_steering_enabled # Flag that enables the wheel steering. +float32 wheel_steering_nudging_rate # [norm] [@range -1, 1] [FRD] Manual wheel nudging, added to controller output. NAN is interpreted as 0. diff --git a/msg/NpfgStatus.msg b/msg/NpfgStatus.msg deleted file mode 100644 index 132c1f7f3f38..000000000000 --- a/msg/NpfgStatus.msg +++ /dev/null @@ -1,17 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -uint8 wind_est_valid # (boolean) true = wind estimate is valid and/or being used by controller (also indicates if wind est usage is disabled despite being valid) -float32 lat_accel # resultant lateral acceleration reference [m/s^2] -float32 lat_accel_ff # lateral acceleration demand only for maintaining curvature [m/s^2] -float32 bearing_feas # bearing feasibility [0,1] -float32 bearing_feas_on_track # on-track bearing feasibility [0,1] -float32 signed_track_error # signed track error [m] -float32 track_error_bound # track error bound [m] -float32 airspeed_ref # (true) airspeed reference [m/s] -float32 bearing # bearing angle [rad] -float32 heading_ref # heading angle reference [rad] -float32 min_ground_speed_ref # minimum forward ground speed reference [m/s] -float32 adapted_period # adapted period (if auto-tuning enabled) [s] -float32 p_gain # controller proportional gain [rad/s] -float32 time_const # controller time constant [s] -float32 can_run_factor # estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1] diff --git a/msg/RateCtrlStatus.msg b/msg/RateCtrlStatus.msg index 3f5644699db0..b1e4d4feb12f 100644 --- a/msg/RateCtrlStatus.msg +++ b/msg/RateCtrlStatus.msg @@ -4,4 +4,3 @@ uint64 timestamp # time since system start (microseconds) float32 rollspeed_integ float32 pitchspeed_integ float32 yawspeed_integ -float32 wheel_rate_integ # FW only and optional diff --git a/msg/px4_msgs_old/msg/VehicleAttitudeSetpointV0.msg b/msg/px4_msgs_old/msg/VehicleAttitudeSetpointV0.msg new file mode 100644 index 000000000000..28aa780699dc --- /dev/null +++ b/msg/px4_msgs_old/msg/VehicleAttitudeSetpointV0.msg @@ -0,0 +1,18 @@ +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +float32 yaw_sp_move_rate # rad/s (commanded by user) + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control + +# For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. +# For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. +float32[3] thrust_body # Normalized thrust command in body FRD frame [-1,1] + +bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change) + +bool fw_control_yaw_wheel # control heading with steering wheel (used for auto takeoff on runway) + +# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 7564a9f83d46..21b128c03c88 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -12,3 +12,4 @@ #include "translation_vehicle_status_v1.h" #include "translation_airspeed_validated_v1.h" +#include "translation_vehicle_attitude_setpoint_v1.h" diff --git a/msg/translation_node/translations/translation_vehicle_attitude_setpoint_v1.h b/msg/translation_node/translations/translation_vehicle_attitude_setpoint_v1.h new file mode 100644 index 000000000000..b30e469dff80 --- /dev/null +++ b/msg/translation_node/translations/translation_vehicle_attitude_setpoint_v1.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * Copyright (c) 2025 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate VehicleAttitudeSetpoint v0 <--> v1 +#include +#include + +class VehicleAttitudeSetpointV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::VehicleAttitudeSetpointV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::VehicleAttitudeSetpoint; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/in/vehicle_attitude_setpoint"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + // Set msg_newer from msg_older + msg_newer.timestamp = msg_older.timestamp; + msg_newer.yaw_sp_move_rate = msg_older.yaw_sp_move_rate; + msg_newer.q_d[0] = msg_older.q_d[0]; + msg_newer.q_d[1] = msg_older.q_d[1]; + msg_newer.q_d[2] = msg_older.q_d[2]; + msg_newer.q_d[3] = msg_older.q_d[3]; + msg_newer.thrust_body[0] = msg_older.thrust_body[0]; + msg_newer.thrust_body[1] = msg_older.thrust_body[1]; + msg_newer.thrust_body[2] = msg_older.thrust_body[2]; + + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + // Set msg_older from msg_newer + msg_older.timestamp = msg_newer.timestamp; + msg_older.yaw_sp_move_rate = msg_newer.yaw_sp_move_rate; + msg_older.q_d[0] = msg_newer.q_d[0]; + msg_older.q_d[1] = msg_newer.q_d[1]; + msg_older.q_d[2] = msg_newer.q_d[2]; + msg_older.q_d[3] = msg_newer.q_d[3]; + msg_older.thrust_body[0] = msg_newer.thrust_body[0]; + msg_older.thrust_body[1] = msg_newer.thrust_body[1]; + msg_older.thrust_body[2] = msg_newer.thrust_body[2]; + + msg_older.reset_integral = false; + msg_older.fw_control_yaw_wheel = false; + + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleAttitudeSetpointV1Translation); diff --git a/msg/versioned/FixedWingLateralSetpoint.msg b/msg/versioned/FixedWingLateralSetpoint.msg new file mode 100644 index 000000000000..ac7410878e23 --- /dev/null +++ b/msg/versioned/FixedWingLateralSetpoint.msg @@ -0,0 +1,11 @@ +# Fixed Wing Lateral Setpoint message +# Used by the fw_lateral_longitudinal_control module +# At least one of course, airspeed_direction, or lateral_acceleration must be finite. + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +float32 course # [rad] [@range -pi, pi] Desired direction of travel over ground w.r.t (true) North. NAN if not controlled directly. +float32 airspeed_direction # [rad] [@range -pi, pi] Desired horizontal angle of airspeed vector w.r.t. (true) North. Same as vehicle heading if in the absence of sideslip. NAN if not controlled directly, takes precedence over course if finite. +float32 lateral_acceleration # [m/s^2] [FRD] Lateral acceleration setpoint. NAN if not controlled directly, used as feedforward if either course setpoint or airspeed_direction is finite. diff --git a/msg/versioned/FixedWingLongitudinalSetpoint.msg b/msg/versioned/FixedWingLongitudinalSetpoint.msg new file mode 100644 index 000000000000..06896df8b68e --- /dev/null +++ b/msg/versioned/FixedWingLongitudinalSetpoint.msg @@ -0,0 +1,14 @@ +# Fixed Wing Longitudinal Setpoint message +# Used by the fw_lateral_longitudinal_control module +# If pitch_direct and throttle_direct are not both finite, then the controller relies on altitude/height_rate and equivalent_airspeed to control vertical motion. +# If both altitude and height_rate are NAN, the controller maintains the current altitude. + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +float32 altitude # [m] Altitude setpoint AMSL, not controlled directly if NAN or if height_rate is finite +float32 height_rate # [m/s] [ENU] Scalar height rate setpoint. NAN if not controlled directly +float32 equivalent_airspeed # [m/s] [@range 0, inf] Scalar equivalent airspeed setpoint. NAN if system default should be used +float32 pitch_direct # [rad] [@range -pi, pi] [FRD] NAN if not controlled, overrides total energy controller +float32 throttle_direct # [norm] [@range 0,1] NAN if not controlled, overrides total energy controller diff --git a/msg/versioned/LateralControlConfiguration.msg b/msg/versioned/LateralControlConfiguration.msg new file mode 100644 index 000000000000..783bd38465ad --- /dev/null +++ b/msg/versioned/LateralControlConfiguration.msg @@ -0,0 +1,8 @@ +# Fixed Wing Lateral Control Configuration message +# Used by the fw_lateral_longitudinal_control module to constrain FixedWingLateralSetpoint messages. + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +float32 lateral_accel_max # [m/s^2] currently maps to a maximum roll angle, accel_max = tan(roll_max) * GRAVITY diff --git a/msg/versioned/LongitudinalControlConfiguration.msg b/msg/versioned/LongitudinalControlConfiguration.msg new file mode 100644 index 000000000000..ac9c56df44d3 --- /dev/null +++ b/msg/versioned/LongitudinalControlConfiguration.msg @@ -0,0 +1,17 @@ +# Fixed Wing Longitudinal Control Configuration message +# Used by the fw_lateral_longitudinal_control module and TECS to constrain FixedWingLongitudinalSetpoint messages +# and configure the resultant setpoints. + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +float32 pitch_min # [rad][@range -pi, pi] defaults to FW_P_LIM_MIN if NAN. +float32 pitch_max # [rad][@range -pi, pi] defaults to FW_P_LIM_MAX if NAN. +float32 throttle_min # [norm] [@range 0,1] deaults to FW_THR_MIN if NAN. +float32 throttle_max # [norm] [@range 0,1] defaults to FW_THR_MAX if NAN. +float32 climb_rate_target # [m/s] target climbrate to change altitude. Defaults to FW_T_CLIMB_MAX if NAN. Not used if height_rate is directly set in FixedWingLongitudinalSetpoint. +float32 sink_rate_target # [m/s] target sinkrate to change altitude. Defaults to FW_T_SINK_MAX if NAN. Not used if height_rate is directly set in FixedWingLongitudinalSetpoint. +float32 speed_weight # [@range 0,2], 0=pitch controls altitude only, 2=pitch controls airspeed only +bool enforce_low_height_condition # [boolean] if true, the altitude controller is configured with an alternative timeconstant for tighter altitude tracking +bool disable_underspeed_protection # [boolean] if true, underspeed handling is disabled in the altitude controller diff --git a/msg/versioned/VehicleAttitudeSetpoint.msg b/msg/versioned/VehicleAttitudeSetpoint.msg index 28aa780699dc..b32b336edb3c 100644 --- a/msg/versioned/VehicleAttitudeSetpoint.msg +++ b/msg/versioned/VehicleAttitudeSetpoint.msg @@ -1,4 +1,4 @@ -uint32 MESSAGE_VERSION = 0 +uint32 MESSAGE_VERSION = 1 uint64 timestamp # time since system start (microseconds) @@ -11,8 +11,4 @@ float32[4] q_d # Desired quaternion for quaternion control # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. float32[3] thrust_body # Normalized thrust command in body FRD frame [-1,1] -bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change) - -bool fw_control_yaw_wheel # control heading with steering wheel (used for auto takeoff on runway) - # TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index b452595732cb..2a00520ab92b 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -29,7 +29,8 @@ flight_mode_manager start mc_pos_control start vtol mc_att_control start vtol mc_rate_control start vtol -fw_pos_control start vtol +fw_mode_manager start +fw_lat_lon_control start fw_att_control start vtol airspeed_selector start @@ -59,7 +60,7 @@ flight_mode_manager status mc_pos_control status mc_att_control status mc_rate_control status -fw_pos_control status +fw_mode_manager status fw_att_control status airspeed_selector status dataman status @@ -74,7 +75,7 @@ mc_att_control stop fw_att_control stop flight_mode_manager stop mc_pos_control stop -fw_pos_control stop +fw_mode_manager stop navigator stop commander stop land_detector stop diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index d6ba875fde14..0d952b84cc13 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -56,7 +56,8 @@ ekf2 start land_detector start fixedwing fw_att_control start -fw_pos_control start +fw_mode_manager start +fw_lat_lon_control start mavlink start -n SoftAp -x -u 14556 -r 1000000 -p # -n name of wifi interface: SoftAp, wlan or your custom interface, diff --git a/posix-configs/rpi/pilotpi_fw.config b/posix-configs/rpi/pilotpi_fw.config index a4fdf0f6fdb6..4c66381e1c52 100644 --- a/posix-configs/rpi/pilotpi_fw.config +++ b/posix-configs/rpi/pilotpi_fw.config @@ -63,7 +63,8 @@ airspeed_selector start land_detector start fixedwing flight_mode_manager start fw_att_control start -fw_pos_control start +fw_mode_manager start +fw_lat_lon_control start mavlink start -x -u 14556 -r 1000000 -p diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index 55d8d9cfd997..5de9619fc8f5 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -41,7 +41,8 @@ navigator start ekf2 start land_detector start fixedwing fw_att_control start -fw_pos_control start +fw_mode_manager start +fw_lat_lon_control start mavlink start -x -u 14556 -r 1000000 -p diff --git a/src/lib/fw_performance_model/PerformanceModel.cpp b/src/lib/fw_performance_model/PerformanceModel.cpp index 6fb315cb8933..e946347a4497 100644 --- a/src/lib/fw_performance_model/PerformanceModel.cpp +++ b/src/lib/fw_performance_model/PerformanceModel.cpp @@ -144,17 +144,18 @@ float PerformanceModel::getCalibratedTrimAirspeed() const return math::constrain(_param_fw_airspd_trim.get() * sqrtf(getWeightRatio()), _param_fw_airspd_min.get(), _param_fw_airspd_max.get()); } -float PerformanceModel::getMinimumCalibratedAirspeed(float load_factor) const +float PerformanceModel::getMinimumCalibratedAirspeed(float load_factor, float flaps_setpoint) const { - load_factor = math::max(load_factor, FLT_EPSILON); - return _param_fw_airspd_min.get() * sqrtf(getWeightRatio() * load_factor); + const float airspeed_flap_factor = math::lerp(1.f, _param_fw_airspd_flp_sc.get(), flaps_setpoint); + return (_param_fw_airspd_min.get() * airspeed_flap_factor) * sqrtf(getWeightRatio() * load_factor); } -float PerformanceModel::getCalibratedStallAirspeed(float load_factor) const +float PerformanceModel::getCalibratedStallAirspeed(float load_factor, float flaps_setpoint) const { load_factor = math::max(load_factor, FLT_EPSILON); - return _param_fw_airspd_stall.get() * sqrtf(getWeightRatio() * load_factor); + const float airspeed_flap_factor = math::lerp(1.f, _param_fw_airspd_flp_sc.get(), flaps_setpoint); + return (_param_fw_airspd_stall.get() * airspeed_flap_factor) * sqrtf(getWeightRatio() * load_factor); } float PerformanceModel::getMaximumCalibratedAirspeed() const diff --git a/src/lib/fw_performance_model/PerformanceModel.hpp b/src/lib/fw_performance_model/PerformanceModel.hpp index fbdd8693c21d..dece1bb3e080 100644 --- a/src/lib/fw_performance_model/PerformanceModel.hpp +++ b/src/lib/fw_performance_model/PerformanceModel.hpp @@ -94,11 +94,12 @@ class PerformanceModel : public ModuleParams float getCalibratedTrimAirspeed() const; /** - * Get the minimum airspeed compensated for weight and load factor due to bank angle. + * Get the minimum airspeed compensated for weight, load factor due to bank angle and flaps. * @param load_factor due to banking + * @param flaps_setpoint Flaps setpoint, normalized in range [0,1] * @return calibrated minimum airspeed in m/s */ - float getMinimumCalibratedAirspeed(float load_factor = 1.0f) const; + float getMinimumCalibratedAirspeed(float load_factor, float flaps_setpoint) const; /** * Get the maximum airspeed. @@ -109,9 +110,10 @@ class PerformanceModel : public ModuleParams /** * get the stall airspeed compensated for load factor due to bank angle. * @param load_factor load factor due to banking + * @param flaps_setpoint Flaps setpoint, normalized in range [0,1] * @return calibrated stall airspeed in m/s */ - float getCalibratedStallAirspeed(float load_factor) const; + float getCalibratedStallAirspeed(float load_factor, float flaps_setpoint) const; /** * Run some checks on parameters and detect unfeasible combinations. @@ -134,7 +136,9 @@ class PerformanceModel : public ModuleParams (ParamFloat) _param_fw_thr_max, (ParamFloat) _param_fw_thr_min, (ParamFloat) _param_fw_thr_aspd_min, - (ParamFloat) _param_fw_thr_aspd_max) + (ParamFloat) _param_fw_thr_aspd_max, + (ParamFloat) _param_fw_airspd_flp_sc + ) /** * Get the sea level trim throttle for a given calibrated airspeed setpoint. diff --git a/src/lib/fw_performance_model/performance_model_params.c b/src/lib/fw_performance_model/performance_model_params.c index 0c39d2ed39c5..2223f60139f9 100644 --- a/src/lib/fw_performance_model/performance_model_params.c +++ b/src/lib/fw_performance_model/performance_model_params.c @@ -212,3 +212,16 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f); * @group FW Performance */ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f); + +/** + * Airspeed scale with full flaps + * + * Factor applied to the minimum and stall airspeed when flaps are fully deployed. + * + * @min 0.5 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group FW Performance + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_FLP_SC, 1.f); diff --git a/src/lib/npfg/AirspeedDirectionController.cpp b/src/lib/npfg/AirspeedDirectionController.cpp new file mode 100644 index 000000000000..621587f47e79 --- /dev/null +++ b/src/lib/npfg/AirspeedDirectionController.cpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "AirspeedDirectionController.hpp" +#include +#include + +using matrix::Vector2f; +AirspeedDirectionController::AirspeedDirectionController() +{ + // Constructor +} + +float AirspeedDirectionController::controlHeading(const float heading_sp, const float heading, + const float airspeed) const +{ + + const Vector2f airspeed_vector = Vector2f{cosf(heading), sinf(heading)} * airspeed; + const Vector2f airspeed_sp_vector_unit = Vector2f{cosf(heading_sp), sinf(heading_sp)}; + + const float dot_air_vel_err = airspeed_vector.dot(airspeed_sp_vector_unit); + const float cross_air_vel_err = airspeed_vector.cross(airspeed_sp_vector_unit); + + if (dot_air_vel_err < 0.0f) { + // hold max lateral acceleration command above 90 deg heading error + return p_gain_ * ((cross_air_vel_err < 0.0f) ? -airspeed : airspeed); + + } else { + // airspeed/airspeed_ref is used to scale any incremented airspeed reference back to the current airspeed + // for acceleration commands in a "feedback" sense (i.e. at the current vehicle airspeed) + // todo use airspeed_ref or adapt comment + return p_gain_ * cross_air_vel_err; + } +} diff --git a/src/lib/npfg/AirspeedDirectionController.hpp b/src/lib/npfg/AirspeedDirectionController.hpp new file mode 100644 index 000000000000..d0111729ca44 --- /dev/null +++ b/src/lib/npfg/AirspeedDirectionController.hpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 AirspeedDirectionController.hpp + * + * Original Author: Thomas Stastny + * Refactored to better suite new control API: Roman Bapst + * + * * Notes: + * - The wind estimate should be dynamic enough to capture ~1-2 second length gusts, + * Otherwise the performance will suffer. + * + * Acknowledgements and References: + * + * The logic is mostly based on [1] and Paper III of [2]. + * TODO: Concise, up to date documentation and stability analysis for the following + * implementation. + * + * [1] T. Stastny and R. Siegwart. "On Flying Backwards: Preventing Run-away of + * Small, Low-speed, Fixed-wing UAVs in Strong Winds". IEEE International Conference + * on Intelligent Robots and Systems (IROS). 2019. + * https://arxiv.org/pdf/1908.01381.pdf + * [2] T. Stastny. "Low-Altitude Control and Local Re-Planning Strategies for Small + * Fixed-Wing UAVs". Doctoral Thesis, ETH Zürich. 2020. + * https://tstastny.github.io/pdf/tstastny_phd_thesis_wcover.pdf + */ + +#ifndef PX4_AIRSPEEDDIRECTIONONTROLLER_HPP +#define PX4_AIRSPEEDDIRECTIONONTROLLER_HPP + +class AirspeedDirectionController +{ +public: + + AirspeedDirectionController(); + + + float controlHeading(const float heading_sp, const float heading, const float airspeed) const; + +private: + float p_gain_{0.8885f}; // proportional gain (computed from period_ and damping_) [rad/s] +}; + +#endif //PX4_AIRSPEEDDIRECTIONONTROLLER_HPP diff --git a/src/lib/npfg/CMakeLists.txt b/src/lib/npfg/CMakeLists.txt index 739e6af15507..6e34a9a5908d 100644 --- a/src/lib/npfg/CMakeLists.txt +++ b/src/lib/npfg/CMakeLists.txt @@ -32,8 +32,13 @@ ############################################################################ px4_add_library(npfg - npfg.cpp - npfg.hpp + DirectionalGuidance.cpp + CourseToAirspeedRefMapper.cpp + AirspeedDirectionController.cpp + DirectionalGuidance.hpp + CourseToAirspeedRefMapper.hpp + AirspeedDirectionController.hpp ) target_link_libraries(npfg PRIVATE geo) +px4_add_unit_gtest(SRC NpfgTest.cpp LINKLIBS npfg) diff --git a/src/lib/npfg/CourseToAirspeedRefMapper.cpp b/src/lib/npfg/CourseToAirspeedRefMapper.cpp new file mode 100644 index 000000000000..170348f1390d --- /dev/null +++ b/src/lib/npfg/CourseToAirspeedRefMapper.cpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "CourseToAirspeedRefMapper.hpp" +using matrix::Vector2f; + +float +CourseToAirspeedRefMapper::mapCourseSetpointToHeadingSetpoint(const float bearing_setpoint, const Vector2f &wind_vel, + float airspeed_sp) const +{ + const Vector2f bearing_vector = Vector2f{cosf(bearing_setpoint), sinf(bearing_setpoint)}; + const float wind_cross_bearing = wind_vel.cross(bearing_vector); + const float wind_dot_bearing = wind_vel.dot(bearing_vector); + + Vector2f air_vel_ref; + + if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_sp, wind_vel.norm())) { + + const float airsp_dot_bearing = projectAirspOnBearing(airspeed_sp, wind_cross_bearing); + air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vector); + + } else { + air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vector, wind_vel.norm(), airspeed_sp); + } + + return atan2f(air_vel_ref(1), air_vel_ref(0)); +} + +float +CourseToAirspeedRefMapper::getMinAirspeedForCurrentBearing(const float bearing_setpoint, const Vector2f &wind_vel, + float airspeed_max, float min_ground_speed) const +{ + const Vector2f bearing_vector = Vector2f{cosf(bearing_setpoint), sinf(bearing_setpoint)}; + const float wind_cross_bearing = wind_vel.cross(bearing_vector); + const float wind_dot_bearing = wind_vel.dot(bearing_vector); + + const bool wind_along_bearing_is_below_min_ground_speed = min_ground_speed > wind_dot_bearing; + + float airspeed_min = 0.f; // return 0 if no min airspeed is necessary + + if (wind_along_bearing_is_below_min_ground_speed) { + // airspeed required to achieve minimum ground speed along bearing vector (5.18) + airspeed_min = sqrtf((min_ground_speed - wind_dot_bearing) * (min_ground_speed - wind_dot_bearing) + + wind_cross_bearing * wind_cross_bearing); + + } + + return math::min(airspeed_min, airspeed_max); +} + +float CourseToAirspeedRefMapper::projectAirspOnBearing(const float airspeed_true, const float wind_cross_bearing) const +{ + // NOTE: wind_cross_bearing must be less than airspeed to use this function + // it is assumed that bearing feasibility is checked and found feasible (e.g. bearingIsFeasible() = true) prior to entering this method + // otherwise the return will be erroneous + + // 3.5.8 + return sqrtf(math::max(airspeed_true * airspeed_true - wind_cross_bearing * wind_cross_bearing, 0.0f)); +} + +int CourseToAirspeedRefMapper::bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, + const float airspeed, const float wind_speed) const +{ + return (fabsf(wind_cross_bearing) < airspeed) && ((wind_dot_bearing > 0.0f) || (wind_speed < airspeed)); +} + +matrix::Vector2f +CourseToAirspeedRefMapper::solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing, + const Vector2f &bearing_vec) const +{ + // essentially a 2D rotation with the speeds (magnitudes) baked in + return Vector2f{airsp_dot_bearing * bearing_vec(0) - wind_cross_bearing * bearing_vec(1), + wind_cross_bearing * bearing_vec(0) + airsp_dot_bearing * bearing_vec(1)}; +} + +matrix::Vector2f CourseToAirspeedRefMapper::infeasibleAirVelRef(const Vector2f &wind_vel, const Vector2f &bearing_vec, + const float wind_speed, const float airspeed) const +{ + // NOTE: wind speed must be greater than airspeed, and airspeed must be greater than zero to use this function + // it is assumed that bearing feasibility is checked and found infeasible (e.g. bearingIsFeasible() = false) prior to entering this method + // otherwise the normalization of the air velocity vector could have a division by zero + Vector2f air_vel_ref = sqrtf(math::max(wind_speed * wind_speed - airspeed * airspeed, 0.0f)) * bearing_vec - wind_vel; + return air_vel_ref.normalized() * airspeed; +} diff --git a/src/lib/npfg/CourseToAirspeedRefMapper.hpp b/src/lib/npfg/CourseToAirspeedRefMapper.hpp new file mode 100644 index 000000000000..65d8e7ae817a --- /dev/null +++ b/src/lib/npfg/CourseToAirspeedRefMapper.hpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 CourseToAirspeedRefMapper.hpp + * + * Original Author: Thomas Stastny + * Refactored to better suite new control API: Roman Bapst + * + * * Notes: + * - The wind estimate should be dynamic enough to capture ~1-2 second length gusts, + * Otherwise the performance will suffer. + * + * Acknowledgements and References: + * + * The logic is mostly based on [1] and Paper III of [2]. + * TODO: Concise, up to date documentation and stability analysis for the following + * implementation. + * + * [1] T. Stastny and R. Siegwart. "On Flying Backwards: Preventing Run-away of + * Small, Low-speed, Fixed-wing UAVs in Strong Winds". IEEE International Conference + * on Intelligent Robots and Systems (IROS). 2019. + * https://arxiv.org/pdf/1908.01381.pdf + * [2] T. Stastny. "Low-Altitude Control and Local Re-Planning Strategies for Small + * Fixed-Wing UAVs". Doctoral Thesis, ETH Zürich. 2020. + * https://tstastny.github.io/pdf/tstastny_phd_thesis_wcover.pdf + */ + +#ifndef PX4_COURSETOAIRSPEEDREFMAPPER_HPP +#define PX4_COURSETOAIRSPEEDREFMAPPER_HPP + + +#include +#include + +class CourseToAirspeedRefMapper +{ +public: + + CourseToAirspeedRefMapper() {}; + + ~CourseToAirspeedRefMapper() = default; + + float mapCourseSetpointToHeadingSetpoint(const float bearing_setpoint, + const matrix::Vector2f &wind_vel, float airspeed_sp) const; + float getMinAirspeedForCurrentBearing(const float bearing_setpoint, + const matrix::Vector2f &wind_vel, float max_airspeed, float min_ground_speed) const; + +private: + /* + * Projection of the air velocity vector onto the bearing line considering + * a connected wind triangle. + * + * @param[in] airspeed Vehicle true airspeed setpoint [m/s] + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @return Projection of air velocity vector on bearing vector [m/s] + */ + float projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const; + /* + * Check for binary bearing feasibility. + * + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] + * @param[in] airspeed Vehicle true airspeed [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @return Binary bearing feasibility: 1 if feasible, 0 if infeasible + */ + int bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, + const float wind_speed) const; + /* + * Air velocity solution for a given wind velocity and bearing vector assuming + * a "high speed" (not backwards) solution in the excess wind case. + * + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @param[in] airsp_dot_bearing 2D dot product of air velocity (solution) and bearing vector [m/s] + * @param[in] bearing_vec Bearing vector + * @return Air velocity reference vector [m/s] + */ + matrix::Vector2f solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing, + const matrix::Vector2f &bearing_vec) const; + /* + * Air velocity solution for an infeasible bearing. + * + * @param[in] wind_vel Wind velocity vector [m/s] + * @param[in] bearing_vec Bearing vector + * @param[in] wind_speed Wind speed [m/s] + * @param[in] airspeed Vehicle true airspeed [m/s] + * @return Air velocity vector [m/s] + */ + matrix::Vector2f infeasibleAirVelRef(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec, + const float wind_speed, const float airspeed) const; +}; + +#endif //PX4_COURSETOAIRSPEEDREFMAPPER_HPP diff --git a/src/lib/npfg/DirectionalGuidance.cpp b/src/lib/npfg/DirectionalGuidance.cpp new file mode 100644 index 000000000000..cd1f7aef93e1 --- /dev/null +++ b/src/lib/npfg/DirectionalGuidance.cpp @@ -0,0 +1,315 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 DirectionalGuidance.cpp + */ +#include "DirectionalGuidance.hpp" +using matrix::Vector2d; +using matrix::Vector2f; + +DirectionalGuidance::DirectionalGuidance() +{ + +} + +DirectionalGuidanceOutput +DirectionalGuidance::guideToPath(const Vector2f &curr_pos_local, const Vector2f &ground_vel, const Vector2f &wind_vel, + const Vector2f &unit_path_tangent, const Vector2f &position_on_path, + const float path_curvature) +{ + const float ground_speed = ground_vel.norm(); + + const Vector2f air_vel = ground_vel - wind_vel; + const float airspeed = air_vel.norm(); + + const float wind_speed = wind_vel.norm(); + + const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path}; + signed_track_error_ = unit_path_tangent.cross(path_pos_to_vehicle); + + // on-track wind triangle projections + const float wind_cross_upt = wind_vel.cross(unit_path_tangent); + const float wind_dot_upt = wind_vel.dot(unit_path_tangent); + + // calculate the bearing feasibility on the track at the current closest point + feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed); + + const float track_error = fabsf(signed_track_error_); + + // update control parameters considering upper and lower stability bounds (if enabled) + // must be called before trackErrorBound() as it updates time_const_ + adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error, + path_curvature, wind_vel, unit_path_tangent, feas_on_track_); + const float time_const = timeConst(adapted_period_, damping_); + + // track error bound is dynamic depending on ground speed + track_error_bound_ = trackErrorBound(ground_speed, time_const); + const float normalized_track_error = normalizedTrackError(track_error, track_error_bound_); + + // look ahead angle based solely on track proximity + const float look_ahead_ang = lookAheadAngle(normalized_track_error); + + track_proximity_ = trackProximity(look_ahead_ang); + + bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error_); + + // wind triangle projections + const float wind_cross_bearing = wind_vel.cross(bearing_vec_); + const float wind_dot_bearing = wind_vel.dot(bearing_vec_); + + // continuous representation of the bearing feasibility + feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed); + + // we consider feasibility of both the current bearing as well as that on the track at the current closest point + const float feas_combined = feas_ * feas_on_track_; + // lateral acceleration needed to stay on curved track (assuming no heading error) + lateral_accel_ff_ = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt, + wind_cross_upt, airspeed, wind_speed, signed_track_error_, path_curvature) * feas_combined * track_proximity_; + course_sp_ = atan2f(bearing_vec_(1), bearing_vec_(0)); + + return DirectionalGuidanceOutput{.course_setpoint = course_sp_, + .lateral_acceleration_feedforward = lateral_accel_ff_}; +} + +float DirectionalGuidance::adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed, + const float track_error, const float path_curvature, const Vector2f &wind_vel, + const Vector2f &unit_path_tangent, const float feas_on_track) const +{ + float period = period_; + const float air_turn_rate = fabsf(path_curvature * airspeed); + const float wind_factor = windFactor(airspeed, wind_speed); + + if (en_period_lb_ && roll_time_const_ > NPFG_EPSILON) { + // lower bound for period not considering path curvature + const float period_lb_zero_curvature = periodLowerBound(0.0f, wind_factor, feas_on_track) * period_safety_factor_; + + // lower bound for period *considering path curvature + float period_lb = periodLowerBound(air_turn_rate, wind_factor, feas_on_track) * period_safety_factor_; + + // calculate the time constant and track error bound considering the zero + // curvature, lower-bounded period and subsequently recalculate the normalized + // track error + const float time_const = timeConst(period_lb_zero_curvature, damping_); + const float track_error_bound = trackErrorBound(ground_speed, time_const); + const float normalized_track_error = normalizedTrackError(track_error, track_error_bound); + + // calculate nominal track proximity with lower bounded time constant + // (only a numerical solution can find corresponding track proximity + // and adapted gains simultaneously) + const float look_ahead_ang = lookAheadAngle(normalized_track_error); + const float track_proximity = trackProximity(look_ahead_ang); + + // linearly ramp in curvature dependent lower bound with track proximity + period_lb = period_lb * track_proximity + (1.0f - track_proximity) * period_lb_zero_curvature; + + // lower bounded period + period = math::max(period_lb, period); + + // only allow upper bounding ONLY if lower bounding is enabled (is otherwise + // dangerous to allow period decrements without stability checks). + // NOTE: if the roll time constant is not accurately known, lower-bound + // checks may be too optimistic and reducing the period can still destabilize + // the system! enable this feature at your own risk. + if (en_period_ub_) { + + const float period_ub = periodUpperBound(air_turn_rate, wind_factor, feas_on_track); + + if (en_period_ub_ && PX4_ISFINITE(period_ub) && period > period_ub) { + // NOTE: if the roll time constant is not accurately known, reducing + // the period here can destabilize the system! + // enable this feature at your own risk! + + // upper bound the period (for track keeping stability), prefer lower bound if violated + const float period_adapted = math::max(period_lb, period_ub); + + // transition from the nominal period to the adapted period as we get + // closer to the track + period = period_adapted * track_proximity + (1.0f - track_proximity) * period; + } + } + } + + return period; +} + +float DirectionalGuidance::normalizedTrackError(const float track_error, const float track_error_bound) const +{ + return math::constrain(track_error / track_error_bound, 0.0f, 1.0f); +} + +float DirectionalGuidance::windFactor(const float airspeed, const float wind_speed) const +{ + // See [TODO: include citation] for definition/elaboration of this approximation. + if (wind_speed > airspeed || airspeed < NPFG_EPSILON) { + return 2.0f; + + } else { + return 2.0f * (1.0f - sqrtf(1.0f - math::min(1.0f, wind_speed / airspeed))); + } +} + +float DirectionalGuidance::periodUpperBound(const float air_turn_rate, const float wind_factor, + const float feas_on_track) const +{ + if (air_turn_rate * wind_factor > NPFG_EPSILON) { + // multiply air turn rate by feasibility on track to zero out when we anyway + // should not consider the curvature + return 4.0f * M_PI_F * damping_ / (air_turn_rate * wind_factor * feas_on_track); + } + + return INFINITY; +} + +float DirectionalGuidance::periodLowerBound(const float air_turn_rate, const float wind_factor, + const float feas_on_track) const +{ + // this method considers a "conservative" lower period bound, i.e. a constant + // worst case bound for any wind ratio, airspeed, and path curvature + + // the lower bound for zero curvature and no wind OR damping ratio < 0.5 + const float period_lb = M_PI_F * roll_time_const_ / damping_; + + if (air_turn_rate * wind_factor < NPFG_EPSILON || damping_ < 0.5f) { + return period_lb; + + } else { + // the lower bound for tracking a curved path in wind with damping ratio > 0.5 + const float period_windy_curved_damped = 4.0f * M_PI_F * roll_time_const_ * damping_; + + // blend the two together as the bearing on track becomes less feasible + return period_windy_curved_damped * feas_on_track + (1.0f - feas_on_track) * period_lb; + } +} + +float DirectionalGuidance::trackProximity(const float look_ahead_ang) const +{ + const float sin_look_ahead_ang = sinf(look_ahead_ang); + return sin_look_ahead_ang * sin_look_ahead_ang; +} + +float DirectionalGuidance::trackErrorBound(const float ground_speed, const float time_const) const +{ + if (ground_speed > 1.0f) { + return ground_speed * time_const; + + } else { + // limit bound to some minimum ground speed to avoid singularities in track + // error normalization. the following equation assumes ground speed minimum = 1.0 + return 0.5f * time_const * (ground_speed * ground_speed + 1.0f); + } +} + +float DirectionalGuidance::timeConst(const float period, const float damping) const +{ + return period * damping; +} + +float DirectionalGuidance::lookAheadAngle(const float normalized_track_error) const +{ + return M_PI_2_F * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f); +} + + +matrix::Vector2f DirectionalGuidance::bearingVec(const Vector2f &unit_path_tangent, const float look_ahead_ang, + const float signed_track_error) const +{ + const float cos_look_ahead_ang = cosf(look_ahead_ang); + const float sin_look_ahead_ang = sinf(look_ahead_ang); + + Vector2f unit_path_normal(-unit_path_tangent(1), unit_path_tangent(0)); // right handed 90 deg (clockwise) turn + Vector2f unit_track_error = -((signed_track_error < 0.0f) ? -1.0f : 1.0f) * unit_path_normal; + + return cos_look_ahead_ang * unit_track_error + sin_look_ahead_ang * unit_path_tangent; +} + +float +DirectionalGuidance::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, + const float wind_speed) const +{ + if (wind_dot_bearing < 0.0f) { + wind_cross_bearing = wind_speed; + + } else { + wind_cross_bearing = fabsf(wind_cross_bearing); + } + + float sin_arg = sinf(M_PI_F * 0.5f * math::constrain((airspeed - wind_cross_bearing) / AIRSPEED_BUFFER, 0.0f, 1.0f)); + return sin_arg * sin_arg; +} + +float DirectionalGuidance::lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &ground_vel, + const float wind_dot_upt, const float wind_cross_upt, const float airspeed, + const float wind_speed, const float signed_track_error, + const float path_curvature) const +{ + // NOTE: all calculations within this function take place at the closet point + // on the path, as if the aircraft were already tracking the given path at + // this point with zero angular error. this allows us to evaluate curvature + // induced requirements for lateral acceleration incrementation and ramp them + // in with the track proximity and further consider the bearing feasibility + // in excess wind conditions (this is done external to this method). + + // path frame curvature is the instantaneous curvature at our current distance + // from the actual path (considering e.g. concentric circles emanating outward/inward) + const float path_frame_curvature = path_curvature / math::max(1.0f - path_curvature * signed_track_error, + fabsf(path_curvature) * MIN_RADIUS); + + // limit tangent ground speed to along track (forward moving) direction + const float tangent_ground_speed = math::max(ground_vel.dot(unit_path_tangent), 0.0f); + + const float path_frame_rate = path_frame_curvature * tangent_ground_speed; + + // speed ratio = projection of ground vel on track / projection of air velocity + // on track + const float speed_ratio = (1.0f + wind_dot_upt / math::max(projectAirspOnBearing(airspeed, wind_cross_upt), + NPFG_EPSILON)); + + // note the use of airspeed * speed_ratio as oppose to ground_speed^2 here -- + // the prior considers that we command lateral acceleration in the air mass + // relative frame while the latter does not + return airspeed * speed_ratio * path_frame_rate; +} + +float DirectionalGuidance::projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const +{ + // NOTE: wind_cross_bearing must be less than airspeed to use this function + // it is assumed that bearing feasibility is checked and found feasible (e.g. bearingIsFeasible() = true) prior to entering this method + // otherwise the return will be erroneous + return sqrtf(math::max(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f)); +} + +float DirectionalGuidance::switchDistance(float wp_radius) const +{ + return math::min(wp_radius, track_error_bound_ * switch_distance_multiplier_); +} diff --git a/src/lib/npfg/npfg.hpp b/src/lib/npfg/DirectionalGuidance.hpp similarity index 51% rename from src/lib/npfg/npfg.hpp rename to src/lib/npfg/DirectionalGuidance.hpp index c36184ef9645..691b1fd4ddbf 100644 --- a/src/lib/npfg/npfg.hpp +++ b/src/lib/npfg/DirectionalGuidance.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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 @@ -32,13 +32,12 @@ ****************************************************************************/ /* - * @file npfg.hpp - * Implementation of a lateral-directional nonlinear path following guidance - * law with excess wind handling. Commands lateral acceleration and airspeed. + * @file DirectionalGuidance.hpp * - * @author Thomas Stastny + * Original Author: Thomas Stastny + * Refactored to better suite new control API: Roman Bapst * - * Notes: + * * Notes: * - The wind estimate should be dynamic enough to capture ~1-2 second length gusts, * Otherwise the performance will suffer. * @@ -57,50 +56,26 @@ * https://tstastny.github.io/pdf/tstastny_phd_thesis_wcover.pdf */ -#ifndef NPFG_H_ -#define NPFG_H_ - +#ifndef PX4_DIRECTIONALGUIDANCE_HPP +#define PX4_DIRECTIONALGUIDANCE_HPP #include #include -#include +struct DirectionalGuidanceOutput { + float course_setpoint{NAN}; + float lateral_acceleration_feedforward{NAN}; +}; -/* - * NPFG - * Lateral-directional nonlinear path following guidance logic with excess wind handling - */ -class NPFG +class DirectionalGuidance { - public: - /** - * @brief Can run - * - * Evaluation if all the necessary information are available such that npfg can produce meaningful results. - * - * @param[in] local_pos is the current vehicle local position uorb message - * @param[in] is_wind_valid flag if the wind estimation is valid - * @return estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1]. Can be used to define proper mitigation actions. - */ - float canRun(const vehicle_local_position_s &local_pos, bool is_wind_valid) const; - /* - * Computes the lateral acceleration and true airspeed references necessary to track - * a path in wind (including excess wind conditions). - * - * @param[in] curr_pos_local Current horizontal vehicle position in local coordinates [m] - * @param[in] ground_vel Vehicle ground velocity vector [m/s] - * @param[in] wind_vel Wind velocity vector [m/s] - * @param[in] unit_path_tangent Unit vector tangent to path at closest point - * in direction of path - * @param[in] position_on_path Horizontal point on the path to track described in local coordinates [m] - * @param[in] path_curvature Path curvature at closest point on track [m^-1] - */ - void guideToPath(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_vel, - const matrix::Vector2f &wind_vel, - const matrix::Vector2f &unit_path_tangent, const matrix::Vector2f &position_on_path, - const float path_curvature); + DirectionalGuidance(); + DirectionalGuidanceOutput guideToPath(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_vel, + const matrix::Vector2f &wind_vel, + const matrix::Vector2f &unit_path_tangent, const matrix::Vector2f &position_on_path, + const float path_curvature); /* * Set the nominal controller period [s]. */ @@ -122,42 +97,6 @@ class NPFG */ void enablePeriodUB(const bool en) { en_period_ub_ = en; } - /* - * Enable minimum forward ground speed maintenance logic. - */ - void enableMinGroundSpeed(const bool en) { en_min_ground_speed_ = en; } - - /* - * Enable track keeping logic in excess wind conditions. - */ - void enableTrackKeeping(const bool en) { en_track_keeping_ = en; } - - /* - * Enable wind excess regulation. Disabling this param disables all airspeed - * reference incrementaion (airspeed reference will always be nominal). - */ - void enableWindExcessRegulation(const bool en) { en_wind_excess_regulation_ = en; } - - /* - * Set the minimum allowed forward ground speed [m/s]. - */ - void setMinGroundSpeed(float min_gsp) { min_gsp_desired_ = math::max(min_gsp, 0.0f); } - - /* - * Set the maximum value of the minimum forward ground speed command for track - * keeping (occurs at the track error boundary) [m/s]. - */ - void setMaxTrackKeepingMinGroundSpeed(float min_gsp) { min_gsp_track_keeping_max_ = math::max(min_gsp, 0.0f); } - - /* - * Set the nominal airspeed reference (true airspeed) [m/s]. - */ - void setAirspeedNom(float airsp) { airspeed_nom_ = math::max(airsp, 0.1f); } - - /* - * Set the maximum airspeed reference (true airspeed) [m/s]. - */ - void setAirspeedMax(float airsp) { airspeed_max_ = math::max(airsp, 0.1f); } /* * Set the autopilot roll response time constant [s]. @@ -165,88 +104,14 @@ class NPFG void setRollTimeConst(float tc) { roll_time_const_ = tc; } /* - * Set the switch distance multiplier. - */ + * Set the switch distance multiplier. + */ void setSwitchDistanceMultiplier(float mult) { switch_distance_multiplier_ = math::max(mult, 0.1f); } /* * Set the period safety factor. */ void setPeriodSafetyFactor(float sf) { period_safety_factor_ = math::max(sf, 1.0f); } - - /* - * @return Controller proportional gain [rad/s] - */ - float getPGain() const { return p_gain_; } - - /* - * @return Controller time constant [s] - */ - float getTimeConst() const { return time_const_; } - - /* - * @return Adapted controller period [s] - */ - float getAdaptedPeriod() const { return adapted_period_; } - - /* - * @return Track error boundary [m] - */ - float getTrackErrorBound() const { return track_error_bound_; } - - /* - * @return Signed track error [m] - */ - float getTrackError() const { return signed_track_error_; } - - /* - * @return Airspeed reference [m/s] - */ - float getAirspeedRef() const { return airspeed_ref_; } - - /* - * @return Heading angle reference [rad] - */ - float getHeadingRef() const { return atan2f(air_vel_ref_(1), air_vel_ref_(0)); } - - /* - * @return Bearing angle [rad] - */ - float getBearing() const { return atan2f(bearing_vec_(1), bearing_vec_(0)); } - - /* - * @return Lateral acceleration command [m/s^2] - */ - float getLateralAccel() const { return lateral_accel_; } - - /* - * @return Feed-forward lateral acceleration command increment for tracking - * path curvature [m/s^2] - */ - float getLateralAccelFF() const { return lateral_accel_ff_; } - - /* - * @return Bearing feasibility [0, 1] - */ - float getBearingFeas() const { return feas_; } - - /* - * @return On-track bearing feasibility [0, 1] - */ - float getOnTrackBearingFeas() const { return feas_on_track_; } - - /* - * @return Minimum forward ground speed reference [m/s] - */ - float getMinGroundSpeedRef() const { return min_ground_speed_ref_; } - - /* - * [Copied directly from ECL_L1_Pos_Controller] - * - * Set the maximum roll angle output in radians - */ - void setRollLimit(float roll_lim_rad) { roll_lim_rad_ = roll_lim_rad; } - /* * [Copied directly from ECL_L1_Pos_Controller] * @@ -259,38 +124,21 @@ class NPFG */ float switchDistance(float wp_radius) const; - /* - * [Copied directly from ECL_L1_Pos_Controller] - * - * Get roll angle setpoint for fixed wing. - * - * @return Roll angle (in NED frame) - */ - float getRollSetpoint() { return roll_setpoint_; } + float getCourseSetpoint() const { return course_sp_; } + float getLateralAccelerationSetpoint() const { return lateral_accel_ff_; } + float getBearingFeasibility() const { return feas_; } + float getBearingFeasibilityOnTrack() const { return feas_on_track_; } + float getSignedTrackError() const { return signed_track_error_; } + float getTrackErrorBound() const { return track_error_bound_; } + float getAdaptedPeriod() const { return adapted_period_; } private: - static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_VALID{3.f}; ///< Factor of velocity standard deviation above which course calculation is considered good enough - static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_INVALID{2.f}; ///< Factor of velocity standard deviation below which course calculation is considered unsafe - static constexpr float COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK{0.09f}; ///< Cos of Heading to track angle below which it is assumed that the vehicle is not pushed back by the wind ~cos(85°) - static constexpr float COS_HEADING_TRACK_ANGLE_PUSHED_BACK{0.f}; ///< Cos of Heading to track angle above which it is assumed that the vehicle is pushed back by the wind - static constexpr float NPFG_EPSILON = 1.0e-6; // small number *bigger than machine epsilon static constexpr float MIN_RADIUS = 0.5f; // minimum effective radius (avoid singularities) [m] - static constexpr float NTE_FRACTION = 0.5f; // normalized track error fraction (must be > 0) - // ^determines at what fraction of the normalized track error the maximum track keeping forward ground speed demand is reached static constexpr float AIRSPEED_BUFFER = 1.5f; // airspeed buffer [m/s] (must be > 0) - // ^the size of the feasibility transition region at cross wind angle >= 90 deg. - // This must be non-zero to avoid jumpy airspeed incrementation while using wind - // excess handling logic. Similarly used as buffer region around zero airspeed. - - /* - * tuning - */ float period_{10.0f}; // nominal (desired) period -- user defined [s] float damping_{0.7071f}; // nominal (desired) damping ratio -- user defined - float p_gain_{0.8885f}; // proportional gain (computed from period_ and damping_) [rad/s] - float time_const_{7.071f}; // time constant (computed from period_ and damping_) [s] float adapted_period_{10.0f}; // auto-adapted period (if stability bounds enabled) [s] /* @@ -300,17 +148,9 @@ class NPFG // guidance options bool en_period_lb_{true}; // enables automatic lower bound constraints on controller period bool en_period_ub_{true}; // enables automatic upper bound constraints on controller period (remains disabled if lower bound is disabled) - bool en_min_ground_speed_{true}; // the airspeed reference is incremented to sustain a user defined minimum forward ground speed - bool en_track_keeping_{false}; // the airspeed reference is incremented to return to the track and sustain zero ground velocity until excess wind subsides - bool en_wind_excess_regulation_{true}; // the airspeed reference is incremented to regulate the excess wind, but not overcome it ... - // ^disabling this parameter disables all other excess wind handling options, using only the nominal airspeed for reference // guidance settings - float airspeed_nom_{15.0f}; // nominal (desired) true airspeed reference (generally equivalent to cruise optimized airspeed) [m/s] - float airspeed_max_{20.0f}; // maximum true airspeed reference - the maximum achievable/allowed airspeed reference [m/s] float roll_time_const_{0.0f}; // autopilot roll response time constant [s] - float min_gsp_desired_{0.0f}; // user defined miminum desired forward ground speed [m/s] - float min_gsp_track_keeping_max_{5.0f}; // maximum, minimum forward ground speed demand from track keeping logic [m/s] // guidance parameters float switch_distance_multiplier_{0.32f}; // a value multiplied by the track error boundary resulting in a lower switch distance @@ -318,12 +158,8 @@ class NPFG float period_safety_factor_{1.5f}; // multiplied by the minimum period for conservative lower bound /* - * internal guidance states - */ - - // speeds - float min_gsp_track_keeping_{0.0f}; // minimum forward ground speed demand from track keeping logic [m/s] - float min_ground_speed_ref_{0.0f}; // resultant minimum forward ground speed reference considering all active guidance logic [m/s] + * internal guidance states + */ //bearing feasibility float feas_{1.0f}; // continous representation of bearing feasibility in [0,1] (0=infeasible, 1=feasible) @@ -332,27 +168,23 @@ class NPFG // track proximity float track_error_bound_{212.13f}; // the current ground speed dependent track error bound [m] float track_proximity_{0.0f}; // value in [0,1] indicating proximity to track, 0 = at track error boundary or beyond, 1 = on track - - // path following states float signed_track_error_{0.0f}; // signed track error [m] matrix::Vector2f bearing_vec_{matrix::Vector2f{1.0f, 0.0f}}; // bearing unit vector + float course_sp_{0.f}; // course setpoint [rad] + float lateral_accel_ff_{0.f}; // lateral acceleration feedforward [m/s^2] /* - * guidance outputs - */ - - float airspeed_ref_{15.0f}; // true airspeed reference [m/s] - matrix::Vector2f air_vel_ref_{matrix::Vector2f{15.0f, 0.0f}}; // true air velocity reference vector [m/s] - float lateral_accel_{0.0f}; // lateral acceleration reference [m/s^2] - float lateral_accel_ff_{0.0f}; // lateral acceleration demand to maintain path curvature [m/s^2] - - /* - * ECL_L1_Pos_Controller functionality + * Cacluates a continuous representation of the bearing feasibility from [0,1]. + * 0 = infeasible, 1 = fully feasible, partial feasibility in between. + * + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] + * @param[in] airspeed Vehicle true airspeed [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @return bearing feasibility */ - - float roll_lim_rad_{math::radians(30.0f)}; // maximum roll angle [rad] - float roll_setpoint_{0.0f}; // current roll angle setpoint [rad] - + float bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, + const float wind_speed) const; /* * Adapts the controller period considering user defined inputs, current flight * condition, path properties, and stability bounds. @@ -371,7 +203,6 @@ class NPFG float adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed, const float track_error, const float path_curvature, const matrix::Vector2f &wind_vel, const matrix::Vector2f &unit_path_tangent, const float feas_on_track) const; - /* * Returns normalized (unitless) and constrained track error [0,1]. * @@ -435,17 +266,6 @@ class NPFG */ float trackErrorBound(const float ground_speed, const float time_const) const; - /* - * Calculates the required controller proportional gain to achieve the desired - * system period and damping ratio. NOTE: actual period and damping will vary - * when following paths with curvature in wind. - * - * @param[in] period Desired system period [s] - * @param[in] damping Desired system damping ratio - * @return Proportional gain [rad/s] - */ - float pGain(const float period, const float damping) const; - /* * Calculates the required controller time constant to achieve the desired * system period and damping ratio. NOTE: actual period and damping will vary @@ -465,7 +285,6 @@ class NPFG * @return Look ahead angle [rad] */ float lookAheadAngle(const float normalized_track_error) const; - /* * Calculates the bearing vector and track proximity transitioning variable * from the look-ahead angle mapping. @@ -481,33 +300,6 @@ class NPFG matrix::Vector2f bearingVec(const matrix::Vector2f &unit_path_tangent, const float look_ahead_ang, const float signed_track_error) const; - /* - * Calculates the minimum forward ground speed demand for minimum forward - * ground speed maintanence as well as track keeping logic. - * - * @param[in] normalized_track_error Normalized track error (track error / track error boundary) - * @param[in] feas Bearing feasibility - * @return Minimum forward ground speed demand [m/s] - */ - float minGroundSpeed(const float normalized_track_error, const float feas); - - /* - * Determines a reference air velocity *without curvature compensation, but - * including "optimal" true airspeed reference compensation in excess wind conditions. - * Nominal and maximum true airspeed member variables must be set before using this method. - * - * @param[in] wind_vel Wind velocity vector [m/s] - * @param[in] bearing_vec Bearing vector - * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] - * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] - * @param[in] wind_speed Wind speed [m/s] - * @param[in] min_ground_speed Minimum commanded forward ground speed [m/s] - * @return Air velocity vector [m/s] - */ - matrix::Vector2f refAirVelocity(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec, - const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed, - const float min_ground_speed) const; - /* * Projection of the air velocity vector onto the bearing line considering * a connected wind triangle. @@ -517,58 +309,6 @@ class NPFG * @return Projection of air velocity vector on bearing vector [m/s] */ float projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const; - - /* - * Check for binary bearing feasibility. - * - * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] - * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] - * @param[in] airspeed Vehicle true airspeed [m/s] - * @param[in] wind_speed Wind speed [m/s] - * @return Binary bearing feasibility: 1 if feasible, 0 if infeasible - */ - int bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, - const float wind_speed) const; - - /* - * Air velocity solution for a given wind velocity and bearing vector assuming - * a "high speed" (not backwards) solution in the excess wind case. - * - * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] - * @param[in] airsp_dot_bearing 2D dot product of air velocity (solution) and bearing vector [m/s] - * @param[in] bearing_vec Bearing vector - * @return Air velocity vector [m/s] - */ - matrix::Vector2f solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing, - const matrix::Vector2f &bearing_vec) const; - - - /* - * Air velocity solution for an infeasible bearing. - * - * @param[in] wind_vel Wind velocity vector [m/s] - * @param[in] bearing_vec Bearing vector - * @param[in] wind_speed Wind speed [m/s] - * @param[in] airspeed Vehicle true airspeed [m/s] - * @return Air velocity vector [m/s] - */ - matrix::Vector2f infeasibleAirVelRef(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec, - const float wind_speed, const float airspeed) const; - - - /* - * Cacluates a continuous representation of the bearing feasibility from [0,1]. - * 0 = infeasible, 1 = fully feasible, partial feasibility in between. - * - * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] - * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] - * @param[in] airspeed Vehicle true airspeed [m/s] - * @param[in] wind_speed Wind speed [m/s] - * @return bearing feasibility - */ - float bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, - const float wind_speed) const; - /* * Calculates an additional feed-forward lateral acceleration demand considering * the path curvature. @@ -588,29 +328,6 @@ class NPFG const float wind_dot_upt, const float wind_cross_upt, const float airspeed, const float wind_speed, const float signed_track_error, const float path_curvature) const; - /* - * Calculates a lateral acceleration demand from the heading error. - * (does not consider path curvature) - * - * @param[in] air_vel Vechile air velocity vector [m/s] - * @param[in] air_vel_ref Reference air velocity vector [m/s] - * @param[in] airspeed Vehicle true airspeed [m/s] - * @return Lateral acceleration demand [m/s^2] - */ - float lateralAccel(const matrix::Vector2f &air_vel, const matrix::Vector2f &air_vel_ref, - const float airspeed) const; - - /******************************************************************************* - * PX4 POSITION SETPOINT INTERFACE FUNCTIONS - */ - - /** - * [Copied directly from ECL_L1_Pos_Controller] - * - * Update roll angle setpoint. This will also apply slew rate limits if set. - */ - void updateRollSetpoint(); - -}; // class NPFG +}; -#endif // NPFG_H_ +#endif //PX4_DIRECTIONALGUIDANCE_HPP diff --git a/src/lib/npfg/NpfgTest.cpp b/src/lib/npfg/NpfgTest.cpp new file mode 100644 index 000000000000..767fcf69b9ee --- /dev/null +++ b/src/lib/npfg/NpfgTest.cpp @@ -0,0 +1,368 @@ + +/**************************************************************************** + * + * Copyright (C) 2025 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. + * + ****************************************************************************/ + +/****************************************************************** + * Test code for the NPFG algorithm + * Run this test only using "make tests TESTFILTER=Npfg" + * + * + * NOTE: + * + * +******************************************************************/ + +#include +#include +#include + +using namespace matrix; + +TEST(NpfgTest, NoWind) +{ + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN + const Vector2f wind_vel(0.f, 0.f); + float bearing = 0.f; + float airspeed_max = 20.f; + float min_ground_speed = 5.0f; + float airspeed_setpoint = 15.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = _course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted); + + // THEN: expect heading due North with a min airspeed equal to min_ground_speed + EXPECT_NEAR(heading_setpoint, 0.f, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, min_ground_speed, FLT_EPSILON); + + // GIVEN: bearing due East + bearing = M_PI_2_F; + airspeed_max = 20.f; + min_ground_speed = 5.0f; + + // WHEN: we update bearing and airspeed magnitude augmentation + min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + + // THEN: expect heading due East with a min airspeed equal to min_ground_speed + EXPECT_NEAR(heading_setpoint, M_PI_2_F, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, min_ground_speed, FLT_EPSILON); +} + +TEST(NpfgTest, LightCrossWind) +{ + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN + const Vector2f wind_vel(0.f, 6.f); + float bearing = 0.f; + float airspeed_max = 20.f; + float min_ground_speed = 5.0f; + float airspeed_setpoint = 15.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading -0.4115168 with a min airspeed of to 7.8 (sqrt(25+36)) + EXPECT_NEAR(heading_setpoint, -0.4115168, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, 7.8f, 0.1f); + + // GIVEN: bearing due South + bearing = M_PI_F; + airspeed_max = 20.f; + min_ground_speed = 5.0f; + + // WHEN: we update bearing and airspeed magnitude augmentation + min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading of -2.73 and a min airspeed of 7.8 (sqrt(25+36)) + EXPECT_NEAR(heading_setpoint, -2.73f, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, 7.8f, 0.1f); +} + +TEST(NpfgTest, StrongHeadWind) +{ + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN: bearing due North and wind from the North + const Vector2f wind_vel(-16.f, 0.f); + float bearing = 0.f; + float airspeed_max = 25.f; + float min_ground_speed = 5.0f; + float airspeed_setpoint = 15.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading due North with a min airspeed equal to 16+min_ground_speed + EXPECT_NEAR(heading_setpoint, 0.f, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, 16 + min_ground_speed, 0.1f); +} + +TEST(NpfgTest, StrongTailWind) +{ + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN: bearing due East and wind from the West + const Vector2f wind_vel(0.f, 16.f); + float bearing = M_PI_2_F; + float airspeed_max = 25.f; + float min_ground_speed = 5.0f; + float airspeed_setpoint = 15.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading due East with a min airspeed at 0 + EXPECT_NEAR(heading_setpoint, M_PI_2_F, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, 0.f, 0.1f); +} + +TEST(NpfgTest, ExcessHeadWind) +{ + // TEST DESCRIPTION: infeasible bearing, with |wind| = |airspeed|. Align with wind + + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN: bearing due North and wind from the North + const Vector2f wind_vel(-25.f, 0.f); + float bearing = 0.f; + float airspeed_max = 25.f; + float min_ground_speed = 5.0f; + float airspeed_setpoint = 15.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading sp due North with a min airspeed equal to airspeed_max + EXPECT_NEAR(heading_setpoint, 0.f, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, airspeed_max, 0.1f); + + // WHEN: we increase the maximum airspeed + airspeed_max = 35.f; + + min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + // THEN: expect the minimum airspeed to be high enough to maintain minimum groundspeed + EXPECT_NEAR(min_airspeed_for_bearing, 30.f, 0.1f); + + // TEST DESCRIPTION: infeasible bearing, with |wind| = |airspeed|. Align with wind + + // GIVEN: bearing east + bearing = M_PI_F / 2.f; + airspeed_max = 25.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading sp due North with a min airspeed equal to airspeed_max + EXPECT_NEAR(heading_setpoint, 0.f, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, airspeed_max, 0.1f); + + // TEST DESCRIPTION: infeasible bearing, with |wind| > |airspeed|. Aircraft should have a heading between the target bearing + // and wind direction to minimize drift while still attempting to reach the bearing. + + // GIVEN: bearing NE + bearing = M_PI_F / 4.f; + airspeed_max = 20.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading setpoint to be between the target bearing and the cross wind + // & the minimum airspeed to be = maximum airspeed + EXPECT_TRUE((heading_setpoint > -M_PI_F / 2.f) && (heading_setpoint < bearing)); + EXPECT_NEAR(min_airspeed_for_bearing, airspeed_max, 0.1f); +} + +TEST(NpfgTest, ExcessTailWind) +{ + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN: bearing due East and wind from the West + const Vector2f wind_vel(0.f, 25.f); + float bearing = M_PI_2_F; + float airspeed_max = 25.f; + float min_ground_speed = 5.0f; + float airspeed_setpoint = 15.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + const float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading due East with a min airspeed equal to 0 + EXPECT_NEAR(heading_setpoint, M_PI_2_F, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, 0.f, 0.1f); +} + +TEST(NpfgTest, ExcessCrossWind) +{ + // TEST DESCRIPTION: infeasible bearing, with |wind| > |airspeed|. Aircraft should have a heading between the target bearing + // and wind direction to minimize drift while still attempting to reach the bearing. + + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN: bearing due North, strong wind due East + const Vector2f wind_vel(0, 30.f); + float bearing = 0.f; + float airspeed_max = 25.f; + float min_ground_speed = 5.f; + float airspeed_setpoint = 15.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading setpoint to be between the target bearing and the cross wind + // & the minimum airspeed to be = maximum airspeed + EXPECT_TRUE((heading_setpoint > -M_PI_F / 2.f) && (heading_setpoint < bearing)); + EXPECT_NEAR(min_airspeed_for_bearing, airspeed_max, 0.1f); + + // TEST DESCRIPTION: infeasible bearing, with |wind| = |airspeed|. Align with wind. + + airspeed_max = 30.f; + + min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + EXPECT_NEAR(heading_setpoint, -M_PI_F / 2.f, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, airspeed_max, 0.1f); +} + +TEST(NpfgTest, HeadingControl) +{ + AirspeedDirectionController _airspeed_reference_controller; + const float p_gain = 0.8885f; + + // GIVEN: that we are already aligned with out heading setpoint + float heading_sp = 0.f; + float heading = 0.f; + float airspeed = 15.f; + + // WHEN: we compute the lateral acceleration setpoint + float lateral_acceleration_setpoint = _airspeed_reference_controller.controlHeading(heading_sp, heading, airspeed); + + // THEN: we expect 0 lateral acceleration + EXPECT_NEAR(lateral_acceleration_setpoint, 0.f, 0.01f); + + // GIVEN: current heading 45 deg NW + heading = - M_PI_F / 4.f; + + // WHEN: we compute the lateral acceleration setpoint + lateral_acceleration_setpoint = _airspeed_reference_controller.controlHeading(heading_sp, heading, airspeed); + + // THEN: we expect a positive lateral acceleration input. = Airspeed vector + // required to correct the difference between the setpoint and the current heading, + // scaled by p_gain. + EXPECT_NEAR(lateral_acceleration_setpoint, airspeed * sinf(heading_sp - heading) * p_gain, 0.01f); + + // GIVEN: current heading 180 (South) + heading = M_PI_F; + + // WHEN: we compute the lateral acceleration setpoint + lateral_acceleration_setpoint = _airspeed_reference_controller.controlHeading(heading_sp, heading, airspeed); + + // THEN: we we expect maxmimum lateral acceleration setpoint + EXPECT_NEAR(lateral_acceleration_setpoint, airspeed * p_gain, 0.01f); +} diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp deleted file mode 100644 index fdcfa570b01c..000000000000 --- a/src/lib/npfg/npfg.cpp +++ /dev/null @@ -1,508 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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 npfg.cpp - * Implementation of a lateral-directional nonlinear path following guidance - * law with excess wind handling. Commands lateral acceleration and airspeed. - * - * Authors and acknowledgements in header. - */ - -#include "npfg.hpp" -#include -#include -#include - -using matrix::Vector2d; -using matrix::Vector2f; - -float NPFG::canRun(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const -{ - if (is_wind_valid) { - // If we have a valid wind estimate, npfg is able to handle all degenerated cases - return 1.f; - } - - // NPFG can run without wind information as long as the system is not flying backwards and has a minimal ground speed - // Check the minimal ground speed. if it is greater than twice the standard deviation, we assume that we can infer a valid track angle - const Vector2f ground_vel(local_pos.vx, local_pos.vy); - const float ground_speed(ground_vel.norm()); - const float low_ground_speed_factor(math::constrain((ground_speed - HORIZONTAL_EVH_FACTOR_COURSE_INVALID * - local_pos.evh) / ((HORIZONTAL_EVH_FACTOR_COURSE_VALID - HORIZONTAL_EVH_FACTOR_COURSE_INVALID)*local_pos.evh), - 0.f, 1.f)); - - // Check that the angle between heading and track is not off too much. if it is greater than 90° we will be pushed back from the wind and the npfg will propably give a roll command in the wrong direction. - const Vector2f heading_vector(matrix::Dcm2f(local_pos.heading)*Vector2f({1.f, 0.f})); - const Vector2f ground_vel_norm(ground_vel.normalized()); - const float flying_forward_factor(math::constrain((heading_vector.dot(ground_vel_norm) - - COS_HEADING_TRACK_ANGLE_PUSHED_BACK) / ((COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK - - COS_HEADING_TRACK_ANGLE_PUSHED_BACK)), - 0.f, 1.f)); - - return flying_forward_factor * low_ground_speed_factor; -} - -void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &ground_vel, const Vector2f &wind_vel, - const Vector2f &unit_path_tangent, - const Vector2f &position_on_path, const float path_curvature) -{ - const float ground_speed = ground_vel.norm(); - - const Vector2f air_vel = ground_vel - wind_vel; - const float airspeed = air_vel.norm(); - - const float wind_speed = wind_vel.norm(); - - const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path}; - signed_track_error_ = unit_path_tangent.cross(path_pos_to_vehicle); - - // on-track wind triangle projections - const float wind_cross_upt = wind_vel.cross(unit_path_tangent); - const float wind_dot_upt = wind_vel.dot(unit_path_tangent); - - // calculate the bearing feasibility on the track at the current closest point - feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed); - - const float track_error = fabsf(signed_track_error_); - - // update control parameters considering upper and lower stability bounds (if enabled) - // must be called before trackErrorBound() as it updates time_const_ - adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error, - path_curvature, wind_vel, unit_path_tangent, feas_on_track_); - p_gain_ = pGain(adapted_period_, damping_); - time_const_ = timeConst(adapted_period_, damping_); - - // track error bound is dynamic depending on ground speed - track_error_bound_ = trackErrorBound(ground_speed, time_const_); - const float normalized_track_error = normalizedTrackError(track_error, track_error_bound_); - - // look ahead angle based solely on track proximity - const float look_ahead_ang = lookAheadAngle(normalized_track_error); - - track_proximity_ = trackProximity(look_ahead_ang); - - bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error_); - - // wind triangle projections - const float wind_cross_bearing = wind_vel.cross(bearing_vec_); - const float wind_dot_bearing = wind_vel.dot(bearing_vec_); - - // continuous representation of the bearing feasibility - feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed); - - // we consider feasibility of both the current bearing as well as that on the track at the current closest point - const float feas_combined = feas_ * feas_on_track_; - - min_ground_speed_ref_ = minGroundSpeed(normalized_track_error, feas_combined); - - // reference air velocity with directional feedforward effect for following - // curvature in wind and magnitude incrementation depending on minimum ground - // speed violations and/or high wind conditions in general - air_vel_ref_ = refAirVelocity(wind_vel, bearing_vec_, wind_cross_bearing, - wind_dot_bearing, wind_speed, min_ground_speed_ref_); - airspeed_ref_ = air_vel_ref_.norm(); - - // lateral acceleration demand based on heading error - const float lateral_accel = lateralAccel(air_vel, air_vel_ref_, airspeed); - - // lateral acceleration needed to stay on curved track (assuming no heading error) - lateral_accel_ff_ = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt, - wind_cross_upt, airspeed, wind_speed, signed_track_error_, path_curvature); - - // total lateral acceleration to drive aircaft towards track as well as account - // for path curvature. The full effect of the feed-forward acceleration is smoothly - // ramped in as the vehicle approaches the track and is further smoothly - // zeroed out as the bearing becomes infeasible. - lateral_accel_ = lateral_accel + feas_combined * track_proximity_ * lateral_accel_ff_; - - updateRollSetpoint(); -} // guideToPath - -float NPFG::adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed, - const float track_error, const float path_curvature, const Vector2f &wind_vel, - const Vector2f &unit_path_tangent, const float feas_on_track) const -{ - float period = period_; - const float air_turn_rate = fabsf(path_curvature * airspeed); - const float wind_factor = windFactor(airspeed, wind_speed); - - if (en_period_lb_ && roll_time_const_ > NPFG_EPSILON) { - // lower bound for period not considering path curvature - const float period_lb_zero_curvature = periodLowerBound(0.0f, wind_factor, feas_on_track) * period_safety_factor_; - - // lower bound for period *considering path curvature - float period_lb = periodLowerBound(air_turn_rate, wind_factor, feas_on_track) * period_safety_factor_; - - // calculate the time constant and track error bound considering the zero - // curvature, lower-bounded period and subsequently recalculate the normalized - // track error - const float time_const = timeConst(period_lb_zero_curvature, damping_); - const float track_error_bound = trackErrorBound(ground_speed, time_const); - const float normalized_track_error = normalizedTrackError(track_error, track_error_bound); - - // calculate nominal track proximity with lower bounded time constant - // (only a numerical solution can find corresponding track proximity - // and adapted gains simultaneously) - const float look_ahead_ang = lookAheadAngle(normalized_track_error); - const float track_proximity = trackProximity(look_ahead_ang); - - // linearly ramp in curvature dependent lower bound with track proximity - period_lb = period_lb * track_proximity + (1.0f - track_proximity) * period_lb_zero_curvature; - - // lower bounded period - period = math::max(period_lb, period); - - // only allow upper bounding ONLY if lower bounding is enabled (is otherwise - // dangerous to allow period decrements without stability checks). - // NOTE: if the roll time constant is not accurately known, lower-bound - // checks may be too optimistic and reducing the period can still destabilize - // the system! enable this feature at your own risk. - if (en_period_ub_) { - - const float period_ub = periodUpperBound(air_turn_rate, wind_factor, feas_on_track); - - if (en_period_ub_ && PX4_ISFINITE(period_ub) && period > period_ub) { - // NOTE: if the roll time constant is not accurately known, reducing - // the period here can destabilize the system! - // enable this feature at your own risk! - - // upper bound the period (for track keeping stability), prefer lower bound if violated - const float period_adapted = math::max(period_lb, period_ub); - - // transition from the nominal period to the adapted period as we get - // closer to the track - period = period_adapted * track_proximity + (1.0f - track_proximity) * period; - } - } - } - - return period; -} // adaptPeriod - -float NPFG::normalizedTrackError(const float track_error, const float track_error_bound) const -{ - return math::constrain(track_error / track_error_bound, 0.0f, 1.0f); -} - -float NPFG::windFactor(const float airspeed, const float wind_speed) const -{ - // See [TODO: include citation] for definition/elaboration of this approximation. - if (wind_speed > airspeed || airspeed < NPFG_EPSILON) { - return 2.0f; - - } else { - return 2.0f * (1.0f - sqrtf(1.0f - math::min(1.0f, wind_speed / airspeed))); - } -} // windFactor - -float NPFG::periodUpperBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const -{ - if (air_turn_rate * wind_factor > NPFG_EPSILON) { - // multiply air turn rate by feasibility on track to zero out when we anyway - // should not consider the curvature - return 4.0f * M_PI_F * damping_ / (air_turn_rate * wind_factor * feas_on_track); - } - - return INFINITY; -} // periodUB - -float NPFG::periodLowerBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const -{ - // this method considers a "conservative" lower period bound, i.e. a constant - // worst case bound for any wind ratio, airspeed, and path curvature - - // the lower bound for zero curvature and no wind OR damping ratio < 0.5 - const float period_lb = M_PI_F * roll_time_const_ / damping_; - - if (air_turn_rate * wind_factor < NPFG_EPSILON || damping_ < 0.5f) { - return period_lb; - - } else { - // the lower bound for tracking a curved path in wind with damping ratio > 0.5 - const float period_windy_curved_damped = 4.0f * M_PI_F * roll_time_const_ * damping_; - - // blend the two together as the bearing on track becomes less feasible - return period_windy_curved_damped * feas_on_track + (1.0f - feas_on_track) * period_lb; - } -} // periodLB - -float NPFG::trackProximity(const float look_ahead_ang) const -{ - const float sin_look_ahead_ang = sinf(look_ahead_ang); - return sin_look_ahead_ang * sin_look_ahead_ang; -} // trackProximity - -float NPFG::trackErrorBound(const float ground_speed, const float time_const) const -{ - if (ground_speed > 1.0f) { - return ground_speed * time_const; - - } else { - // limit bound to some minimum ground speed to avoid singularities in track - // error normalization. the following equation assumes ground speed minimum = 1.0 - return 0.5f * time_const * (ground_speed * ground_speed + 1.0f); - } -} // trackErrorBound - -float NPFG::pGain(const float period, const float damping) const -{ - return 4.0f * M_PI_F * damping / period; -} // pGain - -float NPFG::timeConst(const float period, const float damping) const -{ - return period * damping; -} // timeConst - -float NPFG::lookAheadAngle(const float normalized_track_error) const -{ - return M_PI_2_F * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f); -} // lookAheadAngle - -Vector2f NPFG::bearingVec(const Vector2f &unit_path_tangent, const float look_ahead_ang, - const float signed_track_error) const -{ - const float cos_look_ahead_ang = cosf(look_ahead_ang); - const float sin_look_ahead_ang = sinf(look_ahead_ang); - - Vector2f unit_path_normal(-unit_path_tangent(1), unit_path_tangent(0)); // right handed 90 deg (clockwise) turn - Vector2f unit_track_error = -((signed_track_error < 0.0f) ? -1.0f : 1.0f) * unit_path_normal; - - return cos_look_ahead_ang * unit_track_error + sin_look_ahead_ang * unit_path_tangent; -} // bearingVec - -float NPFG::minGroundSpeed(const float normalized_track_error, const float feas) -{ - // minimum ground speed demand from track keeping logic - min_gsp_track_keeping_ = 0.0f; - - if (en_track_keeping_ && en_wind_excess_regulation_) { - // zero out track keeping speed increment when bearing is feasible - // maximum track keeping speed increment is applied until we are within - // a user defined fraction of the normalized track error - min_gsp_track_keeping_ = (1.0f - feas) * min_gsp_track_keeping_max_ * math::constrain( - normalized_track_error / NTE_FRACTION, 0.0f, - 1.0f); - } - - // minimum ground speed demand from minimum forward ground speed user setting - float min_gsp_desired = 0.0f; - - if (en_min_ground_speed_ && en_wind_excess_regulation_) { - min_gsp_desired = min_gsp_desired_; - } - - return math::max(min_gsp_track_keeping_, min_gsp_desired); -} // minGroundSpeed - -Vector2f NPFG::refAirVelocity(const Vector2f &wind_vel, const Vector2f &bearing_vec, - const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed, - const float min_ground_speed) const -{ - Vector2f air_vel_ref; - - if (min_ground_speed > wind_dot_bearing && (en_min_ground_speed_ || en_track_keeping_) && en_wind_excess_regulation_) { - // minimum ground speed and/or track keeping - - // airspeed required to achieve minimum ground speed along bearing vector - const float airspeed_min = sqrtf((min_ground_speed - wind_dot_bearing) * (min_ground_speed - wind_dot_bearing) + - wind_cross_bearing * wind_cross_bearing); - - if (airspeed_min > airspeed_max_) { - if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_max_, wind_speed)) { - // we will not maintain the minimum ground speed, but can still achieve the bearing at maximum airspeed - const float airsp_dot_bearing = projectAirspOnBearing(airspeed_max_, wind_cross_bearing); - air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); - - } else { - // bearing is maximally infeasible, employ mitigation law - air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_max_); - } - - } else if (airspeed_min > airspeed_nom_) { - // the minimum ground speed is achievable within the nom - max airspeed range - // solve wind triangle with for air velocity reference with minimum airspeed - const float airsp_dot_bearing = projectAirspOnBearing(airspeed_min, wind_cross_bearing); - air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); - - } else { - // the minimum required airspeed is less than nominal, so we can track the bearing and minimum - // ground speed with our nominal airspeed reference - const float airsp_dot_bearing = projectAirspOnBearing(airspeed_nom_, wind_cross_bearing); - air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); - } - - } else { - // wind excess regulation and/or mitigation - - if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_nom_, wind_speed)) { - // bearing is nominally feasible, solve wind triangle for air velocity reference using nominal airspeed - const float airsp_dot_bearing = projectAirspOnBearing(airspeed_nom_, wind_cross_bearing); - air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); - - } else if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_max_, wind_speed) - && en_wind_excess_regulation_) { - // bearing is maximally feasible - if (wind_dot_bearing <= 0.0f) { - // we only increment the airspeed to regulate, but not overcome, excess wind - // NOTE: in the terminal condition, this will result in a zero ground velocity configuration - air_vel_ref = wind_vel; - - } else { - // the bearing is achievable within the nom - max airspeed range - // solve wind triangle with for air velocity reference with minimum airspeed - const float airsp_dot_bearing = 0.0f; // right angle to the bearing line gives minimal airspeed usage - air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); - } - - } else { - // bearing is maximally infeasible, employ mitigation law - const float airspeed_input = (en_wind_excess_regulation_) ? airspeed_max_ : airspeed_nom_; - air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_input); - } - } - - return air_vel_ref; -} // refAirVelocity - -float NPFG::projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const -{ - // NOTE: wind_cross_bearing must be less than airspeed to use this function - // it is assumed that bearing feasibility is checked and found feasible (e.g. bearingIsFeasible() = true) prior to entering this method - // otherwise the return will be erroneous - return sqrtf(math::max(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f)); -} // projectAirspOnBearing - -int NPFG::bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, - const float wind_speed) const -{ - return (fabsf(wind_cross_bearing) < airspeed) && ((wind_dot_bearing > 0.0f) || (wind_speed < airspeed)); -} // bearingIsFeasible - -Vector2f NPFG::solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing, - const Vector2f &bearing_vec) const -{ - // essentially a 2D rotation with the speeds (magnitudes) baked in - return Vector2f{airsp_dot_bearing * bearing_vec(0) - wind_cross_bearing * bearing_vec(1), - wind_cross_bearing * bearing_vec(0) + airsp_dot_bearing * bearing_vec(1)}; -} // solveWindTriangle - -Vector2f NPFG::infeasibleAirVelRef(const Vector2f &wind_vel, const Vector2f &bearing_vec, const float wind_speed, - const float airspeed) const -{ - // NOTE: wind speed must be greater than airspeed, and airspeed must be greater than zero to use this function - // it is assumed that bearing feasibility is checked and found infeasible (e.g. bearingIsFeasible() = false) prior to entering this method - // otherwise the normalization of the air velocity vector could have a division by zero - Vector2f air_vel_ref = sqrtf(math::max(wind_speed * wind_speed - airspeed * airspeed, 0.0f)) * bearing_vec - wind_vel; - return air_vel_ref.normalized() * airspeed; -} // infeasibleAirVelRef - -float NPFG::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, - const float wind_speed) const -{ - if (wind_dot_bearing < 0.0f) { - wind_cross_bearing = wind_speed; - - } else { - wind_cross_bearing = fabsf(wind_cross_bearing); - } - - float sin_arg = sinf(M_PI_F * 0.5f * math::constrain((airspeed - wind_cross_bearing) / AIRSPEED_BUFFER, 0.0f, 1.0f)); - return sin_arg * sin_arg; -} // bearingFeasibility - -float NPFG::lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &ground_vel, - const float wind_dot_upt, const float wind_cross_upt, const float airspeed, - const float wind_speed, const float signed_track_error, const float path_curvature) const -{ - // NOTE: all calculations within this function take place at the closet point - // on the path, as if the aircraft were already tracking the given path at - // this point with zero angular error. this allows us to evaluate curvature - // induced requirements for lateral acceleration incrementation and ramp them - // in with the track proximity and further consider the bearing feasibility - // in excess wind conditions (this is done external to this method). - - // path frame curvature is the instantaneous curvature at our current distance - // from the actual path (considering e.g. concentric circles emanating outward/inward) - const float path_frame_curvature = path_curvature / math::max(1.0f - path_curvature * signed_track_error, - fabsf(path_curvature) * MIN_RADIUS); - - // limit tangent ground speed to along track (forward moving) direction - const float tangent_ground_speed = math::max(ground_vel.dot(unit_path_tangent), 0.0f); - - const float path_frame_rate = path_frame_curvature * tangent_ground_speed; - - // speed ratio = projection of ground vel on track / projection of air velocity - // on track - const float speed_ratio = (1.0f + wind_dot_upt / math::max(projectAirspOnBearing(airspeed, wind_cross_upt), - NPFG_EPSILON)); - - // note the use of airspeed * speed_ratio as oppose to ground_speed^2 here -- - // the prior considers that we command lateral acceleration in the air mass - // relative frame while the latter does not - return airspeed * speed_ratio * path_frame_rate; -} // lateralAccelFF - -float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, const float airspeed) const -{ - // lateral acceleration demand only from the heading error - - const float dot_air_vel_err = air_vel.dot(air_vel_ref); - const float cross_air_vel_err = air_vel.cross(air_vel_ref); - - if (dot_air_vel_err < 0.0f) { - // hold max lateral acceleration command above 90 deg heading error - return p_gain_ * ((cross_air_vel_err < 0.0f) ? -airspeed : airspeed); - - } else { - // airspeed/airspeed_ref is used to scale any incremented airspeed reference back to the current airspeed - // for acceleration commands in a "feedback" sense (i.e. at the current vehicle airspeed) - return p_gain_ * cross_air_vel_err / airspeed_ref_; - } -} // lateralAccel - -float NPFG::switchDistance(float wp_radius) const -{ - return math::min(wp_radius, track_error_bound_ * switch_distance_multiplier_); -} // switchDistance - -void NPFG::updateRollSetpoint() -{ - float roll_new = atanf(lateral_accel_ * 1.0f / CONSTANTS_ONE_G); - roll_new = math::constrain(roll_new, -roll_lim_rad_, roll_lim_rad_); - - if (PX4_ISFINITE(roll_new)) { - roll_setpoint_ = roll_new; - } -} // updateRollSetpoint diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 6a09b8775913..5ff73eb83cf8 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -172,7 +172,12 @@ void TECSAltitudeReferenceModel::update(const float dt, const AltitudeReferenceS bool control_altitude = true; float altitude_setpoint = setpoint.alt; - if (PX4_ISFINITE(setpoint.alt_rate)) { + if (!PX4_ISFINITE(setpoint.alt) && !PX4_ISFINITE(setpoint.alt_rate)) { + // neither altitude nor altitude rate is set - reset to current altitude + _velocity_control_traj_generator.reset(0.f, 0, current_alt); + altitude_setpoint = current_alt; + + } else if (PX4_ISFINITE(setpoint.alt_rate)) { // input is height rate (not altitude) _velocity_control_traj_generator.setCurrentPositionEstimate(current_alt); _velocity_control_traj_generator.update(dt, setpoint.alt_rate); @@ -180,6 +185,7 @@ void TECSAltitudeReferenceModel::update(const float dt, const AltitudeReferenceS control_altitude = PX4_ISFINITE(altitude_setpoint); // returns true if altitude is locked } else { + // input is altitude _velocity_control_traj_generator.reset(0, height_rate, altitude_setpoint); } @@ -463,7 +469,7 @@ void TECSControl::_calcPitchControlUpdate(float dt, const Input &input, const Co if (param.integrator_gain_pitch > FLT_EPSILON) { // Calculate derivative from change in climb angle to rate of change of specific energy balance - const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G; + const float climb_angle_to_SEB_rate = max(input.tas, param.tas_min) * CONSTANTS_ONE_G; // Calculate pitch integrator input term float pitch_integ_input = _getControlError(seb_rate) * param.integrator_gain_pitch / climb_angle_to_SEB_rate; @@ -710,7 +716,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp) { - // Calculate the time since last update (seconds) const hrt_abstime now(hrt_absolute_time()); const float dt = static_cast((now - _update_timestamp)) / 1_s; @@ -781,7 +786,9 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set void TECS::_setFastDescend(const float alt_setpoint, const float alt) { - if (_control_flag.airspeed_enabled && (_fast_descend_alt_err > FLT_EPSILON) + // disable fast descend if we are close to the target altitude or the altitude setpoint is not finite + + if (PX4_ISFINITE(alt_setpoint) && _control_flag.airspeed_enabled && (_fast_descend_alt_err > FLT_EPSILON) && ((alt_setpoint + _fast_descend_alt_err) < alt)) { auto now = hrt_absolute_time(); @@ -792,7 +799,7 @@ void TECS::_setFastDescend(const float alt_setpoint, const float alt) _fast_descend = constrain(max(_fast_descend, static_cast(now - _enabled_fast_descend_timestamp) / static_cast(FAST_DESCEND_RAMP_UP_TIME)), 0.f, 1.f); - } else if ((_fast_descend > FLT_EPSILON) && (_fast_descend_alt_err > FLT_EPSILON)) { + } else if (PX4_ISFINITE(alt_setpoint) && (_fast_descend > FLT_EPSILON) && (_fast_descend_alt_err > FLT_EPSILON)) { // Were in fast descend, scale it down. up until 5m above target altitude _fast_descend = constrain((alt - alt_setpoint - 5.f) / _fast_descend_alt_err, 0.f, 1.f); _enabled_fast_descend_timestamp = 0U; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 9057d792b996..f9328635e6f4 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -47,6 +47,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : { /* fetch initial parameter values */ parameters_update(); + _landing_gear_wheel_pub.advertise(); } FixedwingAttitudeControl::~FixedwingAttitudeControl() @@ -82,6 +83,7 @@ FixedwingAttitudeControl::parameters_update() _wheel_ctrl.set_k_ff(_param_fw_wr_ff.get()); _wheel_ctrl.set_integrator_max(_param_fw_wr_imax.get()); _wheel_ctrl.set_max_rate(radians(_param_fw_w_rmax.get())); + _wheel_ctrl.set_time_constant(0.1f); } void @@ -108,8 +110,6 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); - _att_sp.reset_integral = false; - _att_sp.timestamp = hrt_absolute_time(); _attitude_sp_pub.publish(_att_sp); @@ -269,12 +269,6 @@ void FixedwingAttitudeControl::Run() vehicle_land_detected_poll(); - bool wheel_control = false; - - if (_param_fw_w_en.get() && _att_sp.fw_control_yaw_wheel && _vcontrol_mode.flag_control_auto_enabled) { - wheel_control = true; - } - /* 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) { perf_end(_loop_perf); @@ -283,11 +277,10 @@ void FixedwingAttitudeControl::Run() if (_vcontrol_mode.flag_control_rates_enabled) { - /* Reset integrators if commanded by attitude setpoint, or the aircraft is on ground + /* Reset integrators if the aircraft is on ground * or a multicopter (but not transitioning VTOL or tailsitter) */ - if (_att_sp.reset_integral - || _landed + if (_landed || !_in_fw_or_transition_wo_tailsitter_transition) { _rates_sp.reset_integral = true; @@ -297,27 +290,6 @@ void FixedwingAttitudeControl::Run() _rates_sp.reset_integral = false; } - float groundspeed_scale = 1.f; - - if (wheel_control) { - if (_local_pos_sub.updated()) { - vehicle_local_position_s vehicle_local_position; - - if (_local_pos_sub.copy(&vehicle_local_position)) { - _groundspeed = sqrtf(vehicle_local_position.vx * vehicle_local_position.vx + vehicle_local_position.vy * - vehicle_local_position.vy); - } - } - - // Use stall airspeed to calculate ground speed scaling region. Don't scale below gspd_scaling_trim - float gspd_scaling_trim = (_param_fw_airspd_stall.get()); - - if (_groundspeed > gspd_scaling_trim) { - groundspeed_scale = gspd_scaling_trim / _groundspeed; - - } - } - /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) { @@ -335,13 +307,6 @@ void FixedwingAttitudeControl::Run() _yaw_ctrl.control_yaw(roll_sp, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta(), get_airspeed_constrained()); - if (wheel_control) { - _wheel_ctrl.control_attitude(euler_sp.psi(), euler_angles.psi()); - - } else { - _wheel_ctrl.reset_integrator(); - } - /* Update input data for rate controllers */ Vector3f body_rates_setpoint = Vector3f(_roll_ctrl.get_body_rate_setpoint(), _pitch_ctrl.get_body_rate_setpoint(), _yaw_ctrl.get_body_rate_setpoint()); @@ -382,40 +347,61 @@ void FixedwingAttitudeControl::Run() _rate_sp_pub.publish(_rates_sp); } } + } - // wheel control - float wheel_u = 0.f; + // steering wheel control + fixed_wing_runway_control_s runway_control{}; + _fixed_wing_runway_control_sub.copy(&runway_control); + const bool runway_control_recent = hrt_elapsed_time(&runway_control.timestamp) < 1_s; + const bool wheel_controller_enabled = _param_fw_w_en.get() && _vcontrol_mode.flag_control_auto_enabled + && runway_control_recent && runway_control.wheel_steering_enabled; - if (_vcontrol_mode.flag_control_manual_enabled) { - // always direct control of steering wheel with yaw stick in manual modes - wheel_u = _manual_control_setpoint.yaw; + float groundspeed_scale = 1.f; + float wheel_u = 0.f; - } else { - vehicle_angular_velocity_s angular_velocity{}; - _vehicle_rates_sub.copy(&angular_velocity); - - // XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from - // position controller during auto modes _manual_control_setpoint.r gets passed - // whenever nudging is enabled, otherwise zero - const float wheel_controller_output = _wheel_ctrl.control_bodyrate(dt, angular_velocity.xyz[2], _groundspeed, - groundspeed_scale); - wheel_u = wheel_control ? wheel_controller_output + _att_sp.yaw_sp_move_rate : 0.f; + if (wheel_controller_enabled) { + if (_local_pos_sub.updated()) { + vehicle_local_position_s vehicle_local_position; + + if (_local_pos_sub.copy(&vehicle_local_position)) { + _groundspeed = sqrtf(vehicle_local_position.vx * vehicle_local_position.vx + vehicle_local_position.vy * + vehicle_local_position.vy); + } } - _landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(wheel_u) ? wheel_u : 0.f; - _landing_gear_wheel.timestamp = hrt_absolute_time(); - _landing_gear_wheel_pub.publish(_landing_gear_wheel); + // Use stall airspeed to calculate ground speed scaling region. Don't scale below gspd_scaling_trim + float gspd_scaling_trim = (_param_fw_airspd_stall.get()); - } else { - // full manual - _wheel_ctrl.reset_integrator(); + if (_groundspeed > gspd_scaling_trim) { + groundspeed_scale = gspd_scaling_trim / _groundspeed; + + } - _landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(_manual_control_setpoint.yaw) ? - _manual_control_setpoint.yaw : 0.f; - _landing_gear_wheel.timestamp = hrt_absolute_time(); - _landing_gear_wheel_pub.publish(_landing_gear_wheel); + // set now yaw setpoint once we're entering the first time + if (!PX4_ISFINITE(_steering_wheel_yaw_setpoint)) { + _steering_wheel_yaw_setpoint = euler_angles.psi(); + } + + _wheel_ctrl.control_attitude(_steering_wheel_yaw_setpoint, euler_angles.psi()); + + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_rates_sub.copy(&angular_velocity); + + const float wheel_controller_output = wheel_controller_enabled ? _wheel_ctrl.control_bodyrate(dt, + angular_velocity.xyz[2], _groundspeed, + groundspeed_scale) : 0.f; + wheel_u = wheel_controller_output + runway_control.wheel_steering_nudging_rate; + + } else { + _wheel_ctrl.reset_integrator(); + _steering_wheel_yaw_setpoint = NAN; + wheel_u = _manual_control_setpoint.yaw; // direct yaw stick to wheel steering } + + _landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(wheel_u) ? wheel_u : 0.f; + _landing_gear_wheel.timestamp = hrt_absolute_time(); + _landing_gear_wheel_pub.publish(_landing_gear_wheel); } // backup schedule diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 6c902878e1f6..73a08c00e010 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -100,6 +101,7 @@ class FixedwingAttitudeControl final : public ModuleBase) _param_fw_airspd_max, - (ParamFloat) _param_fw_airspd_min, (ParamFloat) _param_fw_airspd_stall, (ParamFloat) _param_fw_airspd_trim, (ParamBool) _param_fw_use_airspd, diff --git a/src/modules/fw_att_control/fw_wheel_controller.cpp b/src/modules/fw_att_control/fw_wheel_controller.cpp index e26d9a5d3868..3868a9736926 100644 --- a/src/modules/fw_att_control/fw_wheel_controller.cpp +++ b/src/modules/fw_att_control/fw_wheel_controller.cpp @@ -58,7 +58,7 @@ float WheelController::control_bodyrate(float dt, float body_z_rate, float groun if (_k_i > 0.f && groundspeed > 1.f) { // only start integrating when above 1m/s - float id = rate_error * dt * groundspeed_scaler; + float id = rate_error * dt * groundspeed_scaler * groundspeed_scaler; if (_last_output < -1.f) { /* only allow motion to center: increase value */ @@ -74,7 +74,7 @@ float WheelController::control_bodyrate(float dt, float body_z_rate, float groun /* Apply PI rate controller and store non-limited output */ _last_output = _body_rate_setpoint * _k_ff * groundspeed_scaler + - groundspeed_scaler * groundspeed_scaler * (rate_error * _k_p + _integrator); + groundspeed_scaler * groundspeed_scaler * (rate_error * _k_p) + _integrator; return math::constrain(_last_output, -1.f, 1.f); } diff --git a/src/modules/fw_lateral_longitudinal_control/CMakeLists.txt b/src/modules/fw_lateral_longitudinal_control/CMakeLists.txt new file mode 100644 index 000000000000..7642594b7aa3 --- /dev/null +++ b/src/modules/fw_lateral_longitudinal_control/CMakeLists.txt @@ -0,0 +1,15 @@ +set(CONTROL_DEPENDENCIES + npfg + tecs +) + + +px4_add_module( + MODULE modules__fw_lateral_longitudinal_control + MAIN fw_lat_lon_control + SRCS + FwLateralLongitudinalControl.cpp + FwLateralLongitudinalControl.hpp + DEPENDS + ${CONTROL_DEPENDENCIES} +) diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp new file mode 100644 index 000000000000..1c42f5838c21 --- /dev/null +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -0,0 +1,848 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "FwLateralLongitudinalControl.hpp" +#include + +using math::constrain; +using math::max; +using math::radians; + +using matrix::Dcmf; +using matrix::Eulerf; +using matrix::Quatf; +using matrix::Vector2f; + +// [m/s] maximum reference altitude rate threshhold +static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f; +// [us] time after which the wind estimate is disabled if no longer updating +static constexpr hrt_abstime WIND_EST_TIMEOUT = 10_s; +// [s] slew rate with which we change altitude time constant +static constexpr float TECS_ALT_TIME_CONST_SLEW_RATE = 1.0f; + +static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_VALID{3.f}; ///< Factor of velocity standard deviation above which course calculation is considered good enough +static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_INVALID{2.f}; ///< Factor of velocity standard deviation below which course calculation is considered unsafe +static constexpr float COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK{0.09f}; ///< Cos of Heading to track angle below which it is assumed that the vehicle is not pushed back by the wind ~cos(85°) +static constexpr float COS_HEADING_TRACK_ANGLE_PUSHED_BACK{0.f}; ///< Cos of Heading to track angle above which it is assumed that the vehicle is pushed back by the wind + +// [s] Timeout that has to pass in roll-constraining failsafe before warning is triggered +static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s; + +// [-] Can-run threshold needed to trigger the roll-constraining failsafe warning +static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f; + +// [m/s/s] slew rate limit for airspeed setpoint changes +static constexpr float ASPD_SP_SLEW_RATE = 1.f; + +FwLateralLongitudinalControl::FwLateralLongitudinalControl(bool is_vtol) : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _attitude_sp_pub(is_vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + _tecs_status_pub.advertise(); + _flight_phase_estimation_pub.advertise(); + _fixed_wing_lateral_status_pub.advertise(); + parameters_update(); + _airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE); +} + +FwLateralLongitudinalControl::~FwLateralLongitudinalControl() +{ + perf_free(_loop_perf); +} +void +FwLateralLongitudinalControl::parameters_update() +{ + updateParams(); + _performance_model.updateParameters(); + _performance_model.runSanityChecks(); + + _tecs.set_max_climb_rate(_performance_model.getMaximumClimbRate(_air_density)); + _tecs.set_max_sink_rate(_param_fw_t_sink_max.get()); + _tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density)); + _tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed()); + _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed(getLoadFactor(), _flaps_setpoint)); + _tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed()); + _tecs.set_throttle_damp(_param_fw_t_thr_damping.get()); + _tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get()); + _tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get()); + _tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get()); + _tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get()); + _tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get()); + _tecs.set_pitch_damping(_param_fw_t_ptch_damp.get()); + _tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get()); + _tecs.set_fast_descend_altitude_error(_param_fw_t_fast_alt_err.get()); + _tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get()); + _tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get()); + _tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get()); + _tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get()); + _tecs.set_airspeed_measurement_std_dev(_param_speed_standard_dev.get()); + _tecs.set_airspeed_rate_measurement_std_dev(_param_speed_rate_standard_dev.get()); + _tecs.set_airspeed_filter_process_std_dev(_param_process_noise_standard_dev.get()); + + _roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get())); + + _tecs_alt_time_const_slew_rate.setSlewRate(TECS_ALT_TIME_CONST_SLEW_RATE); + _tecs_alt_time_const_slew_rate.setForcedValue(_param_fw_t_h_error_tc.get() * _param_fw_thrtc_sc.get()); +} + +void FwLateralLongitudinalControl::Run() +{ + if (should_exit()) { + _local_pos_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + /* only run controller if position changed */ + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + parameters_update(); + } + + if (_local_pos_sub.update(&_local_pos)) { + + const float control_interval = math::constrain((_local_pos.timestamp - _last_time_loop_ran) * 1e-6f, + 0.001f, 0.1f); + _last_time_loop_ran = _local_pos.timestamp; + + updateControllerConfiguration(); + + _tecs.set_speed_weight(_long_configuration.speed_weight); + updateTECSAltitudeTimeConstant(checkLowHeightConditions() + || _long_configuration.enforce_low_height_condition, control_interval); + _tecs.set_altitude_error_time_constant(_tecs_alt_time_const_slew_rate.getState()); + + if (_vehicle_air_data_sub.updated()) { + _vehicle_air_data_sub.update(); + _air_density = PX4_ISFINITE(_vehicle_air_data_sub.get().rho) ? _vehicle_air_data_sub.get().rho : _air_density; + _tecs.set_max_climb_rate(_performance_model.getMaximumClimbRate(_air_density)); + _tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density)); + } + + if (_vehicle_landed_sub.updated()) { + vehicle_land_detected_s landed{}; + _vehicle_landed_sub.copy(&landed); + _landed = landed.landed; + } + + _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN; + + _vehicle_status_sub.update(); + _control_mode_sub.update(); + + if (_flaps_setpoint_sub.updated()) { + normalized_unsigned_setpoint_s flaps_setpoint{}; + _flaps_setpoint_sub.copy(&flaps_setpoint); + _flaps_setpoint = flaps_setpoint.normalized_setpoint; + } + + update_control_state(); + + if (_control_mode_sub.get().flag_control_manual_enabled && _control_mode_sub.get().flag_control_altitude_enabled + && _local_pos.z_reset_counter != _z_reset_counter) { + if (_control_mode_sub.get().flag_control_altitude_enabled && _local_pos.z_reset_counter != _z_reset_counter) { + // make TECS accept step in altitude and demanded altitude + _tecs.handle_alt_step(_long_control_state.altitude_msl, _long_control_state.height_rate); + } + } + + const bool should_run = (_control_mode_sub.get().flag_control_position_enabled || + _control_mode_sub.get().flag_control_velocity_enabled || + _control_mode_sub.get().flag_control_acceleration_enabled || + _control_mode_sub.get().flag_control_altitude_enabled || + _control_mode_sub.get().flag_control_climb_rate_enabled) && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + || _vehicle_status_sub.get().in_transition_mode); + + if (should_run) { + + // ----- Longitudinal ------ + float pitch_sp{NAN}; + float throttle_sp{NAN}; + + if (_fw_longitudinal_ctrl_sub.updated()) { + _fw_longitudinal_ctrl_sub.copy(&_long_control_sp); + } + + const float airspeed_sp_eas = adapt_airspeed_setpoint(control_interval, _long_control_sp.equivalent_airspeed, + _min_airspeed_from_guidance, _lateral_control_state.wind_speed.length()); + + // If the both altitude and height rate are set, set altitude setpoint to NAN + const float altitude_sp = PX4_ISFINITE(_long_control_sp.height_rate) ? NAN : _long_control_sp.altitude; + + tecs_update_pitch_throttle(control_interval, altitude_sp, + airspeed_sp_eas, + _long_configuration.pitch_min, + _long_configuration.pitch_max, + _long_configuration.throttle_min, + _long_configuration.throttle_max, + _long_configuration.sink_rate_target, + _long_configuration.climb_rate_target, + _long_configuration.disable_underspeed_protection, + _long_control_sp.height_rate + ); + + pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_direct) ? _long_control_sp.pitch_direct : _tecs.get_pitch_setpoint(); + throttle_sp = PX4_ISFINITE(_long_control_sp.throttle_direct) ? _long_control_sp.throttle_direct : + _tecs.get_throttle_setpoint(); + + // ----- Lateral ------ + float roll_sp {NAN}; + + if (_fw_lateral_ctrl_sub.updated()) { + // We store the update of _fw_lateral_ctrl_sub in a member variable instead of only local such that we can run + // the controllers also without new setpoints. + _fw_lateral_ctrl_sub.copy(&_lat_control_sp); + } + + float airspeed_direction_sp{NAN}; + float lateral_accel_sp {NAN}; + const Vector2f airspeed_vector = _lateral_control_state.ground_speed - _lateral_control_state.wind_speed; + + if (PX4_ISFINITE(_lat_control_sp.course) && !PX4_ISFINITE(_lat_control_sp.airspeed_direction)) { + // only use the course setpoint if it's finite but airspeed_direction is not + + airspeed_direction_sp = _course_to_airspeed.mapCourseSetpointToHeadingSetpoint( + _lat_control_sp.course, _lateral_control_state.wind_speed, + airspeed_sp_eas); + + // Note: the here updated _min_airspeed_from_guidance is only used in the next iteration + // in the longitudinal controller. + const float max_true_airspeed = _performance_model.getMaximumCalibratedAirspeed() * _long_control_state.eas2tas; + _min_airspeed_from_guidance = _course_to_airspeed.getMinAirspeedForCurrentBearing( + _lat_control_sp.course, _lateral_control_state.wind_speed, + max_true_airspeed, _param_fw_gnd_spd_min.get()) + / _long_control_state.eas2tas; + + } else if (PX4_ISFINITE(_lat_control_sp.airspeed_direction)) { + // If the airspeed_direction is finite we use that instead of the course. + + airspeed_direction_sp = _lat_control_sp.airspeed_direction; + _min_airspeed_from_guidance = 0.f; // reset if no longer in course control + + } else { + _min_airspeed_from_guidance = 0.f; // reset if no longer in course control + } + + if (PX4_ISFINITE(airspeed_direction_sp)) { + const float heading = atan2f(airspeed_vector(1), airspeed_vector(0)); + lateral_accel_sp = _airspeed_direction_control.controlHeading(airspeed_direction_sp, heading, + airspeed_vector.norm()); + } + + if (PX4_ISFINITE(_lat_control_sp.lateral_acceleration)) { + lateral_accel_sp = PX4_ISFINITE(lateral_accel_sp) ? lateral_accel_sp + _lat_control_sp.lateral_acceleration : + _lat_control_sp.lateral_acceleration; + } + + if (!PX4_ISFINITE(lateral_accel_sp)) { + lateral_accel_sp = 0.f; // mitigation if no valid setpoint is received: 0 lateral acceleration + } + + lateral_accel_sp = getCorrectedLateralAccelSetpoint(lateral_accel_sp); + lateral_accel_sp = math::constrain(lateral_accel_sp, -_lateral_configuration.lateral_accel_max, + _lateral_configuration.lateral_accel_max); + roll_sp = mapLateralAccelerationToRollAngle(lateral_accel_sp); + + fixed_wing_lateral_status_s fixed_wing_lateral_status{}; + fixed_wing_lateral_status.timestamp = hrt_absolute_time(); + fixed_wing_lateral_status.lateral_acceleration_setpoint = lateral_accel_sp; + fixed_wing_lateral_status.can_run_factor = _can_run_factor; + + _fixed_wing_lateral_status_pub.publish(fixed_wing_lateral_status); + + // additional is_finite checks that should not be necessary, but are kept for safety + float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f; + float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f; + const float yaw_body = _yaw; // yaw is not controlled in fixed wing, need to set it though for quaternion generation + const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f; + + if (_control_mode_sub.get().flag_control_manual_enabled) { + roll_body = constrain(roll_body, -radians(_param_fw_r_lim.get()), + radians(_param_fw_r_lim.get())); + pitch_body = constrain(pitch_body, radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get())); + } + + // roll slew rate + roll_body = _roll_slew_rate.update(roll_body, control_interval); + + _att_sp.timestamp = hrt_absolute_time(); + const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); + q.copyTo(_att_sp.q_d); + + _att_sp.thrust_body[0] = thrust_body_x; + + _attitude_sp_pub.publish(_att_sp); + + } + + _z_reset_counter = _local_pos.z_reset_counter; + } + + perf_end(_loop_perf); +} + +void FwLateralLongitudinalControl::updateControllerConfiguration() +{ + if (_lateral_configuration.timestamp == 0) { + _lateral_configuration.timestamp = _local_pos.timestamp; + _lateral_configuration.lateral_accel_max = tanf(radians(_param_fw_r_lim.get())) * CONSTANTS_ONE_G; + + } + + if (_long_configuration.timestamp == 0) { + setDefaultLongitudinalControlConfiguration(); + } + + if (_long_control_configuration_sub.updated() || _parameter_update_sub.updated()) { + longitudinal_control_configuration_s configuration_in{}; + _long_control_configuration_sub.copy(&configuration_in); + updateLongitudinalControlConfiguration(configuration_in); + } + + if (_lateral_control_configuration_sub.updated() || _parameter_update_sub.updated()) { + lateral_control_configuration_s configuration_in{}; + _lateral_control_configuration_sub.copy(&configuration_in); + _lateral_configuration.timestamp = configuration_in.timestamp; + + if (PX4_ISFINITE(configuration_in.lateral_accel_max)) { + _lateral_configuration.lateral_accel_max = min(configuration_in.lateral_accel_max, tanf(radians( + _param_fw_r_lim.get())) * CONSTANTS_ONE_G); + + } else { + _lateral_configuration.lateral_accel_max = tanf(radians(_param_fw_r_lim.get())) * CONSTANTS_ONE_G; + } + } +} + +void +FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, + float pitch_min_rad, float pitch_max_rad, float throttle_min, + float throttle_max, const float desired_max_sinkrate, + const float desired_max_climbrate, + bool disable_underspeed_detection, float hgt_rate_sp) +{ + bool tecs_is_running = true; + + // do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition + if (_vehicle_status_sub.get().is_vtol + && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + || _vehicle_status_sub.get().in_transition_mode)) { + tecs_is_running = false; + return; + + } + + const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min, + throttle_max, airspeed_sp, _air_density); + + _tecs.set_detect_underspeed_enabled(!disable_underspeed_detection); + + // HOTFIX: the airspeed rate estimate using acceleration in body-forward direction has shown to lead to high biases + // when flying tight turns. It's in this case much safer to just set the estimated airspeed rate to 0. + const float airspeed_rate_estimate = 0.f; + + _tecs.update(_long_control_state.pitch_rad - radians(_param_fw_psp_off.get()), + _long_control_state.altitude_msl, + alt_sp, + airspeed_sp, + _long_control_state.airspeed_eas, + _long_control_state.eas2tas, + throttle_min, + throttle_max, + throttle_trim_compensated, + pitch_min_rad - radians(_param_fw_psp_off.get()), + pitch_max_rad - radians(_param_fw_psp_off.get()), + desired_max_climbrate, + desired_max_sinkrate, + airspeed_rate_estimate, + _long_control_state.height_rate, + hgt_rate_sp); + + tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated); + + if (tecs_is_running && !_vehicle_status_sub.get().in_transition_mode + && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { + const TECS::DebugOutput &tecs_output{_tecs.getStatus()}; + + // Check level flight: the height rate setpoint is not set or set to 0 and we are close to the target altitude and target altitude is not moving + if ((fabsf(tecs_output.height_rate_reference) < MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT) && + fabsf(_long_control_state.altitude_msl - tecs_output.altitude_reference) < _param_nav_fw_alt_rad.get()) { + _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_LEVEL; + + } else if (((tecs_output.altitude_reference - _long_control_state.altitude_msl) >= _param_nav_fw_alt_rad.get()) || + (tecs_output.height_rate_reference >= MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) { + _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_CLIMB; + + } else if (((_long_control_state.altitude_msl - tecs_output.altitude_reference) >= _param_nav_fw_alt_rad.get()) || + (tecs_output.height_rate_reference <= -MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) { + _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND; + + } else { + //We can't infer the flight phase , do nothing, estimation is reset at each step + } + } +} + +void +FwLateralLongitudinalControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, + float true_airspeed_derivative_raw, float throttle_trim) +{ + tecs_status_s tecs_status{}; + + const TECS::DebugOutput &debug_output{_tecs.getStatus()}; + + tecs_status.altitude_sp = alt_sp; + tecs_status.altitude_reference = debug_output.altitude_reference; + tecs_status.altitude_time_constant = _tecs.get_altitude_error_time_constant(); + tecs_status.height_rate_reference = debug_output.height_rate_reference; + tecs_status.height_rate_direct = debug_output.height_rate_direct; + tecs_status.height_rate_setpoint = debug_output.control.altitude_rate_control; + tecs_status.height_rate = -_local_pos.vz; + tecs_status.equivalent_airspeed_sp = equivalent_airspeed_sp; + tecs_status.true_airspeed_sp = debug_output.true_airspeed_sp; + tecs_status.true_airspeed_filtered = debug_output.true_airspeed_filtered; + tecs_status.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control; + tecs_status.true_airspeed_derivative = debug_output.true_airspeed_derivative; + tecs_status.true_airspeed_derivative_raw = true_airspeed_derivative_raw; + tecs_status.total_energy_rate = debug_output.control.total_energy_rate_estimate; + tecs_status.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate; + tecs_status.total_energy_rate_sp = debug_output.control.total_energy_rate_sp; + tecs_status.total_energy_balance_rate_sp = debug_output.control.energy_balance_rate_sp; + tecs_status.throttle_integ = debug_output.control.throttle_integrator; + tecs_status.pitch_integ = debug_output.control.pitch_integrator; + tecs_status.throttle_sp = _tecs.get_throttle_setpoint(); + tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint(); + tecs_status.throttle_trim = throttle_trim; + tecs_status.underspeed_ratio = _tecs.get_underspeed_ratio(); + tecs_status.fast_descend_ratio = debug_output.fast_descend; + + tecs_status.timestamp = hrt_absolute_time(); + + _tecs_status_pub.publish(tecs_status); +} + +int FwLateralLongitudinalControl::task_spawn(int argc, char *argv[]) +{ + bool is_vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + is_vtol = true; + } + } + + FwLateralLongitudinalControl *instance = new FwLateralLongitudinalControl(is_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; +} + +bool +FwLateralLongitudinalControl::init() +{ + if (!_local_pos_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +int FwLateralLongitudinalControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int FwLateralLongitudinalControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +fw_lat_lon_control computes attitude and throttle setpoints from lateral and longitudinal control setpoints. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("fw_lat_lon_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +void FwLateralLongitudinalControl::update_control_state() { + updateAltitudeAndHeightRate(); + updateAirspeed(); + updateAttitude(); + updateWind(); + + _lateral_control_state.ground_speed = Vector2f(_local_pos.vx, _local_pos.vy); +} + +void FwLateralLongitudinalControl::updateWind() { + if (_wind_sub.updated()) { + wind_s wind{}; + _wind_sub.update(&wind); + + // assumes wind is valid if finite + _wind_valid = PX4_ISFINITE(wind.windspeed_north) + && PX4_ISFINITE(wind.windspeed_east); + + _time_wind_last_received = hrt_absolute_time(); + + _lateral_control_state.wind_speed(0) = wind.windspeed_north; + _lateral_control_state.wind_speed(1) = wind.windspeed_east; + + } else { + // invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout + _wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT; + } + + if (!_wind_valid) { + _lateral_control_state.wind_speed.setZero(); + } +} + +void FwLateralLongitudinalControl::updateAltitudeAndHeightRate() { + float ref_alt{0.f}; + if (_local_pos.z_global && PX4_ISFINITE(_local_pos.ref_alt)) { + ref_alt = _local_pos.ref_alt; + } + + _long_control_state.altitude_msl = -_local_pos.z + ref_alt; // Altitude AMSL in meters + _long_control_state.height_rate = -_local_pos.vz; + +} + +void FwLateralLongitudinalControl::updateAttitude() { + vehicle_attitude_s att; + + if (_vehicle_attitude_sub.update(&att)) { + + Dcmf R{Quatf(att.q)}; + + // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset + // between multirotor and fixed wing flight + if (_vehicle_status_sub.get().is_vtol_tailsitter) { + const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}}; + R = R * R_offset; + } + + const Eulerf euler_angles(R); + _long_control_state.pitch_rad = euler_angles.theta(); + _yaw = euler_angles.psi(); + + // load factor due to banking + const float load_factor_from_bank_angle = 1.0f / max(cosf(euler_angles.phi()), FLT_EPSILON); + _tecs.set_load_factor(load_factor_from_bank_angle); + } +} + +void FwLateralLongitudinalControl::updateAirspeed() { + + airspeed_validated_s airspeed_validated; + + if (_param_fw_use_airspd.get() && _airspeed_validated_sub.update(&airspeed_validated)) { + + // do not use synthetic airspeed as this would create a thrust loop + if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) + && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) + && airspeed_validated.airspeed_source != airspeed_validated_s::SYNTHETIC) { + + _time_airspeed_last_valid = airspeed_validated.timestamp; + _long_control_state.airspeed_eas = airspeed_validated.calibrated_airspeed_m_s; + _long_control_state.eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f); + } + } + + // no airspeed updates for one second --> declare invalid + const bool airspeed_valid = hrt_elapsed_time(&_time_airspeed_last_valid) < 1_s; + + if (!airspeed_valid) { + _long_control_state.eas2tas = 1.f; + } + + _tecs.enable_airspeed(airspeed_valid); +} + +float +FwLateralLongitudinalControl::adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint, + float calibrated_min_airspeed_guidance, float wind_speed) +{ + float system_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor(), _flaps_setpoint); + + const float system_max_airspeed = _performance_model.getMaximumCalibratedAirspeed(); + + // airspeed setpoint adjustments + if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) { + calibrated_airspeed_setpoint = _performance_model.getCalibratedTrimAirspeed(); + + // Aditional option to increase the min airspeed setpoint based on wind estimate for more stability in higher winds. + if (_wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) { + system_min_airspeed = math::min(system_min_airspeed + _param_fw_wind_arsp_sc.get() * + wind_speed, system_max_airspeed); + } + } + + // increase setpoint to at what's at least required for the lateral guidance + calibrated_airspeed_setpoint = math::max(calibrated_airspeed_setpoint, calibrated_min_airspeed_guidance); + + // constrain airspeed to feasible range + calibrated_airspeed_setpoint = math::constrain(calibrated_airspeed_setpoint, system_min_airspeed, system_max_airspeed); + + if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())) { + + // initialize the airspeed setpoint + if (PX4_ISFINITE(_long_control_state.airspeed_eas) && _long_control_state.airspeed_eas < system_min_airspeed) { + // current airpseed is below minimum - init with minimum + _airspeed_slew_rate_controller.setForcedValue(system_min_airspeed); + + } else if (PX4_ISFINITE(_long_control_state.airspeed_eas) && _long_control_state.airspeed_eas > system_max_airspeed) { + // current airpseed is above maximum - init with maximum + _airspeed_slew_rate_controller.setForcedValue(system_max_airspeed); + + } else if (PX4_ISFINITE(_long_control_state.airspeed_eas)) { + // current airpseed is between min and max - init with current + _airspeed_slew_rate_controller.setForcedValue(_long_control_state.airspeed_eas); + + } else { + // current airpseed is invalid - init with setpoint + _airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint); + } + } else { + // update slew rate state + if (_airspeed_slew_rate_controller.getState() < system_min_airspeed) { + // current airpseed setpoint is below minimum - reset to minimum + _airspeed_slew_rate_controller.setForcedValue(system_min_airspeed); + + } else if (_airspeed_slew_rate_controller.getState() > system_max_airspeed) { + // current airpseed setpoint is above maximum - reset to maximum + _airspeed_slew_rate_controller.setForcedValue(system_max_airspeed); + + } else if (PX4_ISFINITE(_long_control_state.airspeed_eas)) { + // current airpseed setpoint is between min and max - update + _airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval); + + } + } + + return _airspeed_slew_rate_controller.getState(); +} + +bool FwLateralLongitudinalControl::checkLowHeightConditions() const +{ + // Are conditions for low-height + return _param_fw_t_thr_low_hgt.get() >= 0.f && _local_pos.dist_bottom_valid + && _local_pos.dist_bottom < _param_fw_t_thr_low_hgt.get(); +} + +void FwLateralLongitudinalControl::updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt) +{ + // Target time constant for the TECS altitude tracker + float alt_tracking_tc = _param_fw_t_h_error_tc.get(); + + if (is_low_height) { + // If low-height conditions satisfied, compute target time constant for altitude tracking + alt_tracking_tc *= _param_fw_thrtc_sc.get(); + } + + _tecs_alt_time_const_slew_rate.update(alt_tracking_tc, dt); +} + +float FwLateralLongitudinalControl::getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const +{ + if (is_wind_valid) { + // If we have a valid wind estimate, npfg is able to handle all degenerated cases + return 1.f; + } + + // NPFG can run without wind information as long as the system is not flying backwards and has a minimal ground speed + // Check the minimal ground speed. if it is greater than twice the standard deviation, we assume that we can infer a valid track angle + const Vector2f ground_vel(local_pos.vx, local_pos.vy); + const float ground_speed(ground_vel.norm()); + const float low_ground_speed_factor(math::constrain((ground_speed - HORIZONTAL_EVH_FACTOR_COURSE_INVALID * + local_pos.evh) / ((HORIZONTAL_EVH_FACTOR_COURSE_VALID - HORIZONTAL_EVH_FACTOR_COURSE_INVALID)*local_pos.evh), + 0.f, 1.f)); + + // Check that the angle between heading and track is not off too much. if it is greater than 90° we will be pushed back from the wind and the npfg will propably give a roll command in the wrong direction. + const Vector2f heading_vector(matrix::Dcm2f(local_pos.heading)*Vector2f({1.f, 0.f})); + const Vector2f ground_vel_norm(ground_vel.normalized()); + const float flying_forward_factor(math::constrain((heading_vector.dot(ground_vel_norm) - + COS_HEADING_TRACK_ANGLE_PUSHED_BACK) / ((COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK - + COS_HEADING_TRACK_ANGLE_PUSHED_BACK)), + 0.f, 1.f)); + + return flying_forward_factor * low_ground_speed_factor; +} +float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float lateral_accel_sp) +{ + // Scale the npfg output to zero if npfg is not certain for correct output + _can_run_factor = math::constrain(getGuidanceQualityFactor(_local_pos, _wind_valid), 0.f, 1.f); + + hrt_abstime now{hrt_absolute_time()}; + + // Warn the user when the scale is less than 90% for at least 2 seconds (disable in transition) + + // If the npfg was not running before, reset the user warning variables. + if ((now - _time_since_last_npfg_call) > ROLL_WARNING_TIMEOUT) { + _need_report_npfg_uncertain_condition = true; + _time_since_first_reduced_roll = 0U; + } + + if (_vehicle_status_sub.get().in_transition_mode || _can_run_factor > ROLL_WARNING_CAN_RUN_THRESHOLD || _landed) { + // NPFG reports a good condition or we are in transition, reset the user warning variables. + _need_report_npfg_uncertain_condition = true; + _time_since_first_reduced_roll = 0U; + + } else if (_need_report_npfg_uncertain_condition) { + if (_time_since_first_reduced_roll == 0U) { + _time_since_first_reduced_roll = now; + } + + if ((now - _time_since_first_reduced_roll) > ROLL_WARNING_TIMEOUT) { + _need_report_npfg_uncertain_condition = false; + events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning, + "Roll command reduced due to uncertain velocity/wind estimates!"); + } + + } else { + // Nothing to do, already reported. + } + + _time_since_last_npfg_call = now; + + return _can_run_factor * (lateral_accel_sp); +} +float FwLateralLongitudinalControl::mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const { + return atanf(lateral_acceleration_sp / CONSTANTS_ONE_G); +} + +void FwLateralLongitudinalControl::setDefaultLongitudinalControlConfiguration() { + _long_configuration.timestamp = hrt_absolute_time(); + _long_configuration.pitch_min = radians(_param_fw_p_lim_min.get()); + _long_configuration.pitch_max = radians(_param_fw_p_lim_max.get()); + _long_configuration.throttle_min = _param_fw_thr_min.get(); + _long_configuration.throttle_max = _param_fw_thr_max.get(); + _long_configuration.climb_rate_target = _param_climbrate_target.get(); + _long_configuration.sink_rate_target = _param_sinkrate_target.get(); + _long_configuration.disable_underspeed_protection = false; + _long_configuration.enforce_low_height_condition = false; +} + +void FwLateralLongitudinalControl::updateLongitudinalControlConfiguration(const longitudinal_control_configuration_s &configuration_in) { + _long_configuration.timestamp = configuration_in.timestamp; + + if (PX4_ISFINITE(configuration_in.pitch_min)) { + _long_configuration.pitch_min = math::constrain(configuration_in.pitch_min, radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get())); + } else { + _long_configuration.pitch_min = radians(_param_fw_p_lim_min.get()); + } + + if (PX4_ISFINITE(configuration_in.pitch_max)) { + _long_configuration.pitch_max = math::constrain(configuration_in.pitch_max, _long_configuration.pitch_min, radians(_param_fw_p_lim_max.get())); + } else { + _long_configuration.pitch_max = radians(_param_fw_p_lim_max.get()); + } + + if (PX4_ISFINITE(configuration_in.throttle_min)) { + _long_configuration.throttle_min = math::constrain(configuration_in.throttle_min, _param_fw_thr_min.get(), _param_fw_thr_max.get()); + } else { + _long_configuration.throttle_min = _param_fw_thr_min.get(); + } + + if (PX4_ISFINITE(configuration_in.throttle_max)) { + _long_configuration.throttle_max = math::constrain(configuration_in.throttle_max, _long_configuration.throttle_min, _param_fw_thr_max.get()); + } else { + _long_configuration.throttle_max = _param_fw_thr_max.get(); + } + + if (PX4_ISFINITE(configuration_in.climb_rate_target)) { + _long_configuration.climb_rate_target = math::max(0.0f, configuration_in.climb_rate_target); + } else { + _long_configuration.climb_rate_target = _param_climbrate_target.get(); + } + + if (PX4_ISFINITE(configuration_in.sink_rate_target)) { + _long_configuration.sink_rate_target = math::max(0.0f, configuration_in.sink_rate_target); + } else { + _long_configuration.sink_rate_target = _param_sinkrate_target.get(); + } +} + +float FwLateralLongitudinalControl::getLoadFactor() const +{ + float load_factor_from_bank_angle = 1.f; + + const float roll_body = Eulerf(Quatf(_att_sp.q_d)).phi(); + + if (PX4_ISFINITE(roll_body)) { + load_factor_from_bank_angle = 1.f / math::max(cosf(roll_body), FLT_EPSILON); + } + + return load_factor_from_bank_angle; +} + +extern "C" __EXPORT int fw_lat_lon_control_main(int argc, char *argv[]) +{ + return FwLateralLongitudinalControl::main(argc, argv); +} diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp new file mode 100644 index 000000000000..590054ae8988 --- /dev/null +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp @@ -0,0 +1,264 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 FwLateralLongitudinalControl.hpp + */ + +#ifndef PX4_FWLATERALLONGITUDINALCONTROL_HPP +#define PX4_FWLATERALLONGITUDINALCONTROL_HPP + +#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 +#include +#include + +static constexpr fixed_wing_lateral_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course = NAN, .airspeed_direction = NAN, .lateral_acceleration = NAN}; +static constexpr fixed_wing_longitudinal_setpoint_s empty_longitudinal_control_setpoint = {.timestamp = 0, .altitude = NAN, .height_rate = NAN, .equivalent_airspeed = NAN, .pitch_direct = NAN, .throttle_direct = NAN}; + +class FwLateralLongitudinalControl final : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + FwLateralLongitudinalControl(bool is_vtol); + + ~FwLateralLongitudinalControl() 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; + + uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)}; + uORB::Subscription _wind_sub{ORB_ID(wind)}; + uORB::SubscriptionData _control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::SubscriptionData _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_landed_sub{ORB_ID(vehicle_land_detected)}; + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _fw_lateral_ctrl_sub{ORB_ID(fixed_wing_lateral_setpoint)}; + uORB::Subscription _fw_longitudinal_ctrl_sub{ORB_ID(fixed_wing_longitudinal_setpoint)}; + uORB::Subscription _long_control_configuration_sub{ORB_ID(longitudinal_control_configuration)}; + uORB::Subscription _lateral_control_configuration_sub{ORB_ID(lateral_control_configuration)}; + + vehicle_local_position_s _local_pos{}; + fixed_wing_longitudinal_setpoint_s _long_control_sp{empty_longitudinal_control_setpoint}; + longitudinal_control_configuration_s _long_configuration{}; + fixed_wing_lateral_setpoint_s _lat_control_sp{empty_lateral_control_setpoint}; + lateral_control_configuration_s _lateral_configuration{}; + + float _flaps_setpoint{0.f}; + + uORB::Publication _attitude_sp_pub; + uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; + uORB::PublicationData _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)}; + uORB::Publication _fixed_wing_lateral_status_pub{ORB_ID(fixed_wing_lateral_status)}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_fw_psp_off, + (ParamBool) _param_fw_use_airspd, + (ParamFloat) _param_nav_fw_alt_rad, + (ParamFloat) _param_fw_r_lim, + (ParamFloat) _param_fw_p_lim_max, + (ParamFloat) _param_fw_p_lim_min, + (ParamFloat) _param_fw_pn_r_slew_max, + (ParamFloat) _param_fw_t_hrate_ff, + (ParamFloat) _param_fw_t_h_error_tc, + (ParamFloat) _param_fw_t_fast_alt_err, + (ParamFloat) _param_fw_t_thr_integ, + (ParamFloat) _param_fw_t_I_gain_pit, + (ParamFloat) _param_fw_t_ptch_damp, + (ParamFloat) _param_fw_t_rll2thr, + (ParamFloat) _param_fw_t_sink_max, + (ParamFloat) _param_fw_t_tas_error_tc, + (ParamFloat) _param_fw_t_thr_damping, + (ParamFloat) _param_fw_t_vert_acc, + (ParamFloat) _param_ste_rate_time_const, + (ParamFloat) _param_seb_rate_ff, + (ParamFloat) _param_speed_standard_dev, + (ParamFloat) _param_speed_rate_standard_dev, + (ParamFloat) _param_process_noise_standard_dev, + (ParamFloat) _param_climbrate_target, + (ParamFloat) _param_sinkrate_target, + (ParamFloat) _param_fw_thr_max, + (ParamFloat) _param_fw_thr_min, + (ParamFloat) _param_fw_thr_slew_max, + (ParamFloat) _param_fw_thrtc_sc, + (ParamFloat) _param_fw_t_thr_low_hgt, + (ParamFloat) _param_fw_wind_arsp_sc, + (ParamFloat) _param_fw_gnd_spd_min + ) + + hrt_abstime _last_time_loop_ran{}; + uint8_t _z_reset_counter{UINT8_C(0)}; + uint64_t _time_airspeed_last_valid{UINT64_C(0)}; + float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos}; + // Smooths changes in the altitude tracking error time constant value + SlewRate _tecs_alt_time_const_slew_rate; + struct longitudinal_control_state { + float pitch_rad{0.f}; + float altitude_msl{0.f}; + float airspeed_eas{0.f}; + float eas2tas{1.f}; + float height_rate{0.f}; + } _long_control_state{}; + + bool _wind_valid{false}; + hrt_abstime _time_wind_last_received{0}; + SlewRate _roll_slew_rate; + float _yaw{0.f}; + struct lateral_control_state { + matrix::Vector2f ground_speed; + matrix::Vector2f wind_speed; + } _lateral_control_state{}; + bool _need_report_npfg_uncertain_condition{false}; ///< boolean if reporting of uncertain npfg output condition is needed + hrt_abstime _time_since_first_reduced_roll{0U}; ///< absolute time since start when entering reduced roll angle for the first time + hrt_abstime _time_since_last_npfg_call{0U}; ///< absolute time since start when the npfg reduced roll angle calculations was last performed + vehicle_attitude_setpoint_s _att_sp{}; + bool _landed{false}; + float _can_run_factor{0.f}; + SlewRate _airspeed_slew_rate_controller; + + perf_counter_t _loop_perf; // loop performance counter + + PerformanceModel _performance_model; + TECS _tecs; + CourseToAirspeedRefMapper _course_to_airspeed; + AirspeedDirectionController _airspeed_direction_control; + + float _min_airspeed_from_guidance{0.f}; // need to store it bc we only update after running longitudinal controller + + void parameters_update(); + void update_control_state(); + void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, + float pitch_min_rad, float pitch_max_rad, float throttle_min, + float throttle_max, const float desired_max_sinkrate, + const float desired_max_climbrate, + bool disable_underspeed_detection, float hgt_rate_sp); + + void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw, + float throttle_trim); + + void updateAirspeed(); + + void updateAttitude(); + + void updateAltitudeAndHeightRate(); + + float mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const; + + void updateWind(); + + void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt); + + bool checkLowHeightConditions() const; + + float getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const; + + float getCorrectedLateralAccelSetpoint(float lateral_accel_sp); + + void setDefaultLongitudinalControlConfiguration(); + + void updateLongitudinalControlConfiguration(const longitudinal_control_configuration_s &configuration_in); + + void updateControllerConfiguration(); + + float getLoadFactor() const; + + /** + * @brief Returns an adapted calibrated airspeed setpoint + * + * Adjusts the setpoint for wind, accelerated stall, and slew rates. + * + * @param control_interval Time since the last position control update [s] + * @param calibrated_airspeed_setpoint Calibrated airspeed septoint (generally from the position setpoint) [m/s] + * @param calibrated_min_airspeed_guidance Minimum airspeed required for lateral guidance [m/s] + * @return Adjusted calibrated airspeed setpoint [m/s] + */ + float adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint, + float calibrated_min_airspeed_guidance, float wind_speed); +}; + +#endif //PX4_FWLATERALLONGITUDINALCONTROL_HPP diff --git a/src/modules/fw_lateral_longitudinal_control/Kconfig b/src/modules/fw_lateral_longitudinal_control/Kconfig new file mode 100644 index 000000000000..aefa5a7559fc --- /dev/null +++ b/src/modules/fw_lateral_longitudinal_control/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_FW_LATERAL_LONGITUDINAL_CONTROL + bool "fw_lateral_longitudinal_control" + default n + ---help--- + Enable support for fw_lat_lon_control diff --git a/src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.c b/src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.c new file mode 100644 index 000000000000..0856e8c12384 --- /dev/null +++ b/src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.c @@ -0,0 +1,284 @@ +/** + * Path navigation roll slew rate limit. + * + * Maximum change in roll angle setpoint per second. + * Applied in all Auto modes, plus manual Position & Altitude modes. + * + * @unit deg/s + * @min 0 + * @decimal 0 + * @increment 1 + * @group FW Lateral Control + */ +PARAM_DEFINE_FLOAT(FW_PN_R_SLEW_MAX, 90.0f); + +/** + * Minimum groundspeed + * + * The controller will increase the commanded airspeed to maintain + * this minimum groundspeed to the next waypoint. + * + * @unit m/s + * @min 0.0 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f); + + + + +// ----------longitudinal params---------- + +/** + * Throttle max slew rate + * + * Maximum slew rate for the commanded throttle + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f); + +/** + * Low-height threshold for tighter altitude tracking + * + * Height above ground threshold below which tighter altitude + * tracking gets enabled (see FW_LND_THRTC_SC). Below this height, TECS smoothly + * (1 sec / sec) transitions the altitude tracking time constant from FW_T_ALT_TC + * to FW_LND_THRTC_SC*FW_T_ALT_TC. + * + * -1 to disable. + * + * @unit m + * @min -1 + * @decimal 0 + * @increment 1 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_THR_LOW_HGT, -1.f); + +/** + * Throttle damping factor + * + * This is the damping gain for the throttle demand loop. + * + * @min 0.0 + * @max 1.0 + * @decimal 3 + * @increment 0.01 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_THR_DAMPING, 0.05f); + +/** + * Integrator gain throttle + * + * Increase it to trim out speed and height offsets faster, + * with the downside of possible overshoots and oscillations. + * + * @min 0.0 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_THR_INTEG, 0.02f); + +/** + * Integrator gain pitch + * + * Increase it to trim out speed and height offsets faster, + * with the downside of possible overshoots and oscillations. + * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.05 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f); + +/** + * Maximum vertical acceleration + * + * This is the maximum vertical acceleration + * either up or down that the controller will use to correct speed + * or height errors. + * + * @unit m/s^2 + * @min 1.0 + * @max 10.0 + * @decimal 1 + * @increment 0.5 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); + +/** + * Airspeed measurement standard deviation + * + * For the airspeed filter in TECS. + * + * @unit m/s + * @min 0.01 + * @max 10.0 + * @decimal 2 + * @increment 0.1 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_SPD_STD, 0.07f); + +/** + * Airspeed rate measurement standard deviation + * + * For the airspeed filter in TECS. + * + * @unit m/s^2 + * @min 0.01 + * @max 10.0 + * @decimal 2 + * @increment 0.1 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_SPD_DEV_STD, 0.2f); + +/** + * Process noise standard deviation for the airspeed rate + * + * This is defining the noise in the airspeed rate for the constant airspeed rate model + * of the TECS airspeed filter. + * + * @unit m/s^2 + * @min 0.01 + * @max 10.0 + * @decimal 2 + * @increment 0.1 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_SPD_PRC_STD, 0.2f); + +/** + * Roll -> Throttle feedforward + * + * Is used to compensate for the additional drag created by turning. + * Increase this gain if the aircraft initially loses energy in turns + * and reduce if the aircraft initially gains energy in turns. + * + * @min 0.0 + * @max 20.0 + * @decimal 1 + * @increment 0.5 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f); + +/** + * Pitch damping gain + * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.1 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.1f); + +/** + * Altitude error time constant. + * + * @min 2.0 + * @decimal 2 + * @increment 0.5 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_ALT_TC, 5.0f); + +/** + * Fast descend: minimum altitude error + * + * Minimum altitude error needed to descend with max airspeed and minimal throttle. + * A negative value disables fast descend. + * + * @min -1.0 + * @decimal 0 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_F_ALT_ERR, -1.0f); + +/** + * Height rate feed forward + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.3f); + +/** + * True airspeed error time constant. + * + * @min 2.0 + * @decimal 2 + * @increment 0.5 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_TAS_TC, 5.0f); + +/** + * Specific total energy rate first order filter time constant. + * + * This filter is applied to the specific total energy rate used for throttle damping. + * + * @min 0.0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_STE_R_TC, 0.4f); + +/** + * Specific total energy balance rate feedforward gain. + * + * + * @min 0.5 + * @max 3 + * @decimal 2 + * @increment 0.01 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_SEB_R_FF, 1.0f); + +/** + * Wind-based airspeed scaling factor + * + * Multiplying this factor with the current absolute wind estimate gives the airspeed offset + * added to the minimum airspeed setpoint limit. This helps to make the + * system more robust against disturbances (turbulence) in high wind. + * + * @min 0 + * @decimal 2 + * @increment 0.01 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.f); + +/** + * Maximum descent rate + * + * @unit m/s + * @min 1.0 + * @max 15.0 + * @decimal 1 + * @increment 0.5 + * @group FW Longitudinal Control + */ +PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); diff --git a/src/modules/fw_pos_control/CMakeLists.txt b/src/modules/fw_mode_manager/CMakeLists.txt similarity index 89% rename from src/modules/fw_pos_control/CMakeLists.txt rename to src/modules/fw_mode_manager/CMakeLists.txt index 9248e8d2df5a..3e21a51ee1b3 100644 --- a/src/modules/fw_pos_control/CMakeLists.txt +++ b/src/modules/fw_mode_manager/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2025 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 @@ -55,11 +55,13 @@ endif() px4_add_module( - MODULE modules__fw_pos_control - MAIN fw_pos_control + MODULE modules__fw_mode_manager + MAIN fw_mode_manager SRCS - FixedwingPositionControl.cpp - FixedwingPositionControl.hpp + FixedWingModeManager.cpp + FixedWingModeManager.hpp + ControllerConfigurationHandler.cpp + ControllerConfigurationHandler.hpp DEPENDS ${POSCONTROL_DEPENDENCIES} ) diff --git a/src/modules/fw_mode_manager/ControllerConfigurationHandler.cpp b/src/modules/fw_mode_manager/ControllerConfigurationHandler.cpp new file mode 100644 index 000000000000..fcfabb167923 --- /dev/null +++ b/src/modules/fw_mode_manager/ControllerConfigurationHandler.cpp @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 CombinedControllerConfigurationHandler.cpp + */ + +#include "ControllerConfigurationHandler.hpp" +#include + +using namespace time_literals; + + +void CombinedControllerConfigurationHandler::update(const hrt_abstime now) +{ + _longitudinal_updated = floatValueChanged(_longitudinal_configuration_current_cycle.pitch_min, + _longitudinal_publisher.get().pitch_min); + _longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.pitch_max, + _longitudinal_publisher.get().pitch_max); + _longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.throttle_min, + _longitudinal_publisher.get().throttle_min); + _longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.throttle_max, + _longitudinal_publisher.get().throttle_max); + _longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.speed_weight, + _longitudinal_publisher.get().speed_weight); + _longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.climb_rate_target, + _longitudinal_publisher.get().climb_rate_target); + _longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.sink_rate_target, + _longitudinal_publisher.get().sink_rate_target); + _longitudinal_updated |= booleanValueChanged(_longitudinal_configuration_current_cycle.enforce_low_height_condition, + _longitudinal_publisher.get().enforce_low_height_condition); + _longitudinal_updated |= booleanValueChanged(_longitudinal_configuration_current_cycle.disable_underspeed_protection, + _longitudinal_publisher.get().disable_underspeed_protection); + + _lateral_updated |= floatValueChanged(_lateral_configuration_current_cycle.lateral_accel_max, + _lateral_publisher.get().lateral_accel_max); + + if (_longitudinal_updated || now - _time_last_longitudinal_publish > 1_s) { + _longitudinal_configuration_current_cycle.timestamp = now; + _longitudinal_publisher.update(_longitudinal_configuration_current_cycle); + _time_last_longitudinal_publish = _longitudinal_configuration_current_cycle.timestamp; + } + + if (_lateral_updated || now - _time_last_lateral_publish > 1_s) { + _lateral_configuration_current_cycle.timestamp = now; + _lateral_publisher.update(_lateral_configuration_current_cycle); + _time_last_lateral_publish = _lateral_configuration_current_cycle.timestamp; + } + + _longitudinal_updated = _lateral_updated = false; + _longitudinal_configuration_current_cycle = empty_longitudinal_control_configuration; + _lateral_configuration_current_cycle = empty_lateral_control_configuration; +} + +void CombinedControllerConfigurationHandler::setThrottleMax(float throttle_max) +{ + _longitudinal_configuration_current_cycle.throttle_max = throttle_max; +} + +void CombinedControllerConfigurationHandler::setThrottleMin(float throttle_min) +{ + _longitudinal_configuration_current_cycle.throttle_min = throttle_min; +} + +void CombinedControllerConfigurationHandler::setSpeedWeight(float speed_weight) +{ + _longitudinal_configuration_current_cycle.speed_weight = speed_weight; +} + +void CombinedControllerConfigurationHandler::setPitchMin(const float pitch_min) +{ + _longitudinal_configuration_current_cycle.pitch_min = pitch_min; +} + +void CombinedControllerConfigurationHandler::setPitchMax(const float pitch_max) +{ + _longitudinal_configuration_current_cycle.pitch_max = pitch_max; +} + +void CombinedControllerConfigurationHandler::setClimbRateTarget(float climbrate_target) +{ + _longitudinal_configuration_current_cycle.climb_rate_target = climbrate_target; +} + +void CombinedControllerConfigurationHandler::setDisableUnderspeedProtection(bool disable) +{ + _longitudinal_configuration_current_cycle.disable_underspeed_protection = disable; +} + +void CombinedControllerConfigurationHandler::setSinkRateTarget(const float sinkrate_target) +{ + _longitudinal_configuration_current_cycle.sink_rate_target = sinkrate_target; +} + +void CombinedControllerConfigurationHandler::setLateralAccelMax(float lateral_accel_max) +{ + _lateral_configuration_current_cycle.lateral_accel_max = lateral_accel_max; +} + +void CombinedControllerConfigurationHandler::setEnforceLowHeightCondition(bool low_height_condition) +{ + _longitudinal_configuration_current_cycle.enforce_low_height_condition = low_height_condition; +} + +void CombinedControllerConfigurationHandler::resetLastPublishTime() +{ + _time_last_longitudinal_publish = _time_last_lateral_publish = 0; +} diff --git a/src/modules/fw_mode_manager/ControllerConfigurationHandler.hpp b/src/modules/fw_mode_manager/ControllerConfigurationHandler.hpp new file mode 100644 index 000000000000..73cfc78e4b02 --- /dev/null +++ b/src/modules/fw_mode_manager/ControllerConfigurationHandler.hpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 CombinedControllerConfigurationHandler.hpp + */ +#ifndef PX4_CONTROLLERCONFIGURATIONHANDLER_HPP +#define PX4_CONTROLLERCONFIGURATIONHANDLER_HPP + +#include +#include +#include +#include + +static constexpr longitudinal_control_configuration_s empty_longitudinal_control_configuration = {.timestamp = 0, .pitch_min = NAN, .pitch_max = NAN, .throttle_min = NAN, .throttle_max = NAN, .climb_rate_target = NAN, .sink_rate_target = NAN, .speed_weight = NAN, .enforce_low_height_condition = false, .disable_underspeed_protection = false }; +static constexpr lateral_control_configuration_s empty_lateral_control_configuration = {.timestamp = 0, .lateral_accel_max = NAN}; + + +class CombinedControllerConfigurationHandler +{ +public: + CombinedControllerConfigurationHandler() = default; + ~CombinedControllerConfigurationHandler() = default; + + void update(const hrt_abstime now); + + void setEnforceLowHeightCondition(bool low_height_condition); + + void setThrottleMax(float throttle_max); + + void setThrottleMin(float throttle_min); + + void setSpeedWeight(float speed_weight); + + void setPitchMin(const float pitch_min); + + void setPitchMax(const float pitch_max); + + void setClimbRateTarget(float climbrate_target); + + void setDisableUnderspeedProtection(bool disable); + + void setSinkRateTarget(const float sinkrate_target); + + void setLateralAccelMax(float lateral_accel_max); + + void resetLastPublishTime(); + +private: + bool booleanValueChanged(bool new_value, bool current_value) + { + return current_value != new_value; + } + + bool floatValueChanged(float new_value, float current_value) + { + bool diff; + + if (PX4_ISFINITE(new_value)) { + diff = PX4_ISFINITE(current_value) ? (fabsf(new_value - current_value) > FLT_EPSILON) : true; + + } else { + diff = PX4_ISFINITE(current_value); + } + + return diff; + } + + bool _lateral_updated{false}; + bool _longitudinal_updated{false}; + + hrt_abstime _time_last_longitudinal_publish{0}; + hrt_abstime _time_last_lateral_publish{0}; + + uORB::PublicationData _lateral_publisher{ORB_ID(lateral_control_configuration)}; + uORB::PublicationData _longitudinal_publisher{ORB_ID(longitudinal_control_configuration)}; + + lateral_control_configuration_s _lateral_configuration_current_cycle{empty_lateral_control_configuration}; + longitudinal_control_configuration_s _longitudinal_configuration_current_cycle {empty_longitudinal_control_configuration}; +}; + +#endif //PX4_CONTROLLERCONFIGURATIONHANDLER_HPP diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp similarity index 50% rename from src/modules/fw_pos_control/FixedwingPositionControl.cpp rename to src/modules/fw_mode_manager/FixedWingModeManager.cpp index de00c74b597e..57eef702d85a 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2025 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 @@ -31,9 +31,10 @@ * ****************************************************************************/ -#include "FixedwingPositionControl.hpp" +#include "FixedWingModeManager.hpp" #include +#include using math::constrain; using math::max; @@ -48,50 +49,40 @@ using matrix::Vector2d; using matrix::Vector3f; using matrix::wrap_pi; -FixedwingPositionControl::FixedwingPositionControl(bool vtol) : +const fixed_wing_lateral_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course = NAN, .airspeed_direction = NAN, .lateral_acceleration = NAN}; +const fixed_wing_longitudinal_setpoint_s empty_longitudinal_control_setpoint = {.timestamp = 0, .altitude = NAN, .height_rate = NAN, .equivalent_airspeed = NAN, .pitch_direct = NAN, .throttle_direct = NAN}; + +FixedWingModeManager::FixedWingModeManager() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), - _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _launchDetector(this), _runway_takeoff(this) #ifdef CONFIG_FIGURE_OF_EIGHT - , _figure_eight(_npfg, _wind_vel, _eas2tas) + , _figure_eight(_directional_guidance, _wind_vel) #endif // CONFIG_FIGURE_OF_EIGHT { - // limit to 50 Hz _local_pos_sub.set_interval_ms(20); - _pos_ctrl_status_pub.advertise(); _pos_ctrl_landing_status_pub.advertise(); - _tecs_status_pub.advertise(); _launch_detection_status_pub.advertise(); _landing_gear_pub.advertise(); - _flaps_setpoint_pub.advertise(); _spoilers_setpoint_pub.advertise(); + _fixed_wing_lateral_guidance_status_pub.advertise(); + _fixed_wing_runway_control_pub.advertise(); - _airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE); - - /* fetch initial parameter values */ parameters_update(); - - _roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get())); - _roll_slew_rate.setForcedValue(0.f); - - _tecs_alt_time_const_slew_rate.setSlewRate(TECS_ALT_TIME_CONST_SLEW_RATE); - _tecs_alt_time_const_slew_rate.setForcedValue(_param_fw_t_h_error_tc.get() * _param_fw_thrtc_sc.get()); - } -FixedwingPositionControl::~FixedwingPositionControl() +FixedWingModeManager::~FixedWingModeManager() { perf_free(_loop_perf); } bool -FixedwingPositionControl::init() +FixedWingModeManager::init() { if (!_local_pos_sub.registerCallback()) { PX4_ERR("callback registration failed"); @@ -102,59 +93,21 @@ FixedwingPositionControl::init() } void -FixedwingPositionControl::parameters_update() +FixedWingModeManager::parameters_update() { updateParams(); - _performance_model.updateParameters(); - - _roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get())); - - // NPFG parameters - _npfg.setPeriod(_param_npfg_period.get()); - _npfg.setDamping(_param_npfg_damping.get()); - _npfg.enablePeriodLB(_param_npfg_en_period_lb.get()); - _npfg.enablePeriodUB(_param_npfg_en_period_ub.get()); - _npfg.enableMinGroundSpeed(_param_npfg_en_min_gsp.get()); - _npfg.enableTrackKeeping(_param_npfg_en_track_keeping.get()); - _npfg.enableWindExcessRegulation(_param_npfg_en_wind_reg.get()); - _npfg.setMinGroundSpeed(_param_fw_gnd_spd_min.get()); - _npfg.setMaxTrackKeepingMinGroundSpeed(_param_npfg_track_keeping_gsp_max.get()); - _npfg.setRollTimeConst(_param_npfg_roll_time_const.get()); - _npfg.setSwitchDistanceMultiplier(_param_npfg_switch_distance_multiplier.get()); - _npfg.setRollLimit(radians(_param_fw_r_lim.get())); - _npfg.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get()); - - // TECS parameters - _tecs.set_max_climb_rate(_performance_model.getMaximumClimbRate(_air_density)); - _tecs.set_max_sink_rate(_param_fw_t_sink_max.get()); - _tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density)); - _tecs.set_speed_weight(_param_fw_t_spdweight.get()); - _tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed()); - _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); - _tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed()); - _tecs.set_throttle_damp(_param_fw_t_thr_damping.get()); - _tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get()); - _tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get()); - _tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get()); - _tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get()); - _tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get()); - _tecs.set_pitch_damping(_param_fw_t_ptch_damp.get()); - _tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get()); - _tecs.set_fast_descend_altitude_error(_param_fw_t_fast_alt_err.get()); - _tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get()); - _tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get()); - _tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get()); - _tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get()); - _tecs.set_airspeed_measurement_std_dev(_param_speed_standard_dev.get()); - _tecs.set_airspeed_rate_measurement_std_dev(_param_speed_rate_standard_dev.get()); - _tecs.set_airspeed_filter_process_std_dev(_param_process_noise_standard_dev.get()); - - _performance_model.runSanityChecks(); + _directional_guidance.setPeriod(_param_npfg_period.get()); + _directional_guidance.setDamping(_param_npfg_damping.get()); + _directional_guidance.enablePeriodLB(_param_npfg_en_period_lb.get()); + _directional_guidance.enablePeriodUB(_param_npfg_en_period_ub.get()); + _directional_guidance.setRollTimeConst(_param_npfg_roll_time_const.get()); + _directional_guidance.setSwitchDistanceMultiplier(_param_npfg_switch_distance_multiplier.get()); + _directional_guidance.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get()); } void -FixedwingPositionControl::vehicle_control_mode_poll() +FixedWingModeManager::vehicle_control_mode_poll() { if (_control_mode_sub.updated()) { const bool was_armed = _control_mode.flag_armed; @@ -171,7 +124,7 @@ FixedwingPositionControl::vehicle_control_mode_poll() } void -FixedwingPositionControl::vehicle_command_poll() +FixedWingModeManager::vehicle_command_poll() { vehicle_command_s vehicle_command; @@ -205,47 +158,27 @@ FixedwingPositionControl::vehicle_command_poll() } void -FixedwingPositionControl::airspeed_poll() +FixedWingModeManager::airspeed_poll() { - bool airspeed_valid = _airspeed_valid; airspeed_validated_s airspeed_validated; if (_param_fw_use_airspd.get() && _airspeed_validated_sub.update(&airspeed_validated)) { - _eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed - - // do not use synthetic airspeed as this would create a thrust loop + // do not use synthetic airspeed as it's for the use here not reliable enough if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) - && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) && airspeed_validated.airspeed_source != airspeed_validated_s::SYNTHETIC) { - airspeed_valid = true; - - _time_airspeed_last_valid = airspeed_validated.timestamp; _airspeed_eas = airspeed_validated.calibrated_airspeed_m_s; - - _eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f); - - } else { - airspeed_valid = false; - } - - } else { - // no airspeed updates for one second - if (airspeed_valid && (hrt_elapsed_time(&_time_airspeed_last_valid) > 1_s)) { - airspeed_valid = false; } } - // update TECS if validity changed - if (airspeed_valid != _airspeed_valid) { - _tecs.enable_airspeed(airspeed_valid); - _airspeed_valid = airspeed_valid; - } + // no airspeed updates for one second --> declare invalid + // this flag is used for some logic like: exiting takeoff, flaps retraction + _airspeed_valid = hrt_elapsed_time(&_time_airspeed_last_valid) < 1_s; } void -FixedwingPositionControl::wind_poll() +FixedWingModeManager::wind_poll(const hrt_abstime now) { if (_wind_sub.updated()) { wind_s wind; @@ -255,14 +188,14 @@ FixedwingPositionControl::wind_poll() _wind_valid = PX4_ISFINITE(wind.windspeed_north) && PX4_ISFINITE(wind.windspeed_east); - _time_wind_last_received = hrt_absolute_time(); + _time_wind_last_received = now; _wind_vel(0) = wind.windspeed_north; _wind_vel(1) = wind.windspeed_east; } else { // invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout - _wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT; + _wind_valid = _wind_valid && (now - _time_wind_last_received) < WIND_EST_TIMEOUT; } if (!_wind_valid) { @@ -272,7 +205,7 @@ FixedwingPositionControl::wind_poll() } void -FixedwingPositionControl::manual_control_setpoint_poll() +FixedWingModeManager::manual_control_setpoint_poll() { _manual_control_setpoint_sub.update(&_manual_control_setpoint); @@ -294,18 +227,17 @@ FixedwingPositionControl::manual_control_setpoint_poll() } } - void -FixedwingPositionControl::vehicle_attitude_poll() +FixedWingModeManager::vehicle_attitude_poll() { - vehicle_attitude_s att; + vehicle_attitude_s vehicle_attitude; - if (_vehicle_attitude_sub.update(&att)) { + if (_vehicle_attitude_sub.update(&vehicle_attitude)) { vehicle_angular_velocity_s angular_velocity{}; _vehicle_angular_velocity_sub.copy(&angular_velocity); const Vector3f rates{angular_velocity.xyz}; - Dcmf R{Quatf(att.q)}; + Dcmf R{Quatf(vehicle_attitude.q)}; // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset // between multirotor and fixed wing flight @@ -320,184 +252,37 @@ FixedwingPositionControl::vehicle_attitude_poll() } const Eulerf euler_angles(R); - _pitch = euler_angles(1); _yaw = euler_angles(2); - Vector3f body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az}; + const Vector3f body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az}; _body_acceleration_x = body_acceleration(0); - Vector3f body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz}; + const Vector3f body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz}; _body_velocity_x = body_velocity(0); - - // load factor due to banking - _tecs.set_load_factor(getLoadFactor()); } } float -FixedwingPositionControl::get_manual_airspeed_setpoint() +FixedWingModeManager::get_manual_airspeed_setpoint() { - - float altctrl_airspeed = _performance_model.getCalibratedTrimAirspeed(); + float manual_airspeed_setpoint = NAN; if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) { // neutral throttle corresponds to trim airspeed - return math::interpolateNXY(_manual_control_setpoint_for_airspeed, + manual_airspeed_setpoint = math::interpolateNXY(_manual_control_setpoint_for_airspeed, {-1.f, 0.f, 1.f}, - {_performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), altctrl_airspeed, _performance_model.getMaximumCalibratedAirspeed()}); + {_param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_airspd_max.get()}); } else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) { - altctrl_airspeed = constrain(_commanded_manual_airspeed_setpoint, - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), - _performance_model.getMaximumCalibratedAirspeed()); + // override stick by commanded airspeed + manual_airspeed_setpoint = _commanded_manual_airspeed_setpoint; } - return altctrl_airspeed; -} - -float -FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint, - float calibrated_min_airspeed, const Vector2f &ground_speed, bool in_takeoff_situation) -{ - // --- airspeed *constraint adjustments --- - - // Aditional option to increase the min airspeed setpoint based on wind estimate for more stability in higher winds. - if (!in_takeoff_situation && _airspeed_valid && _wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) { - calibrated_min_airspeed = math::min(calibrated_min_airspeed + _param_fw_wind_arsp_sc.get() * - _wind_vel.length(), _performance_model.getMaximumCalibratedAirspeed()); - } - - // --- airspeed *setpoint adjustments --- - - if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) { - calibrated_airspeed_setpoint = _performance_model.getCalibratedTrimAirspeed(); - } - - // Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained - if (!_wind_valid && !in_takeoff_situation) { - /* - * This error value ensures that a plane (as long as its throttle capability is - * not exceeded) travels towards a waypoint (and is not pushed more and more away - * by wind). Not countering this would lead to a fly-away. Only non-zero in presence - * of sufficient wind. "minimum ground speed undershoot". - */ - const float ground_speed_body = _body_velocity_x; - - if (ground_speed_body < _param_fw_gnd_spd_min.get()) { - calibrated_airspeed_setpoint += _param_fw_gnd_spd_min.get() - ground_speed_body; - } - } - - calibrated_airspeed_setpoint = constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed, - _performance_model.getMaximumCalibratedAirspeed()); - - // initialize the airspeed setpoint to the max(current airsped, min airspeed) - if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState()) - || !_tecs_is_running) { - _airspeed_slew_rate_controller.setForcedValue(math::max(calibrated_min_airspeed, _airspeed_eas)); - } - - // reset the airspeed setpoint to the min airspeed if the min airspeed changes while in operation - if (_airspeed_slew_rate_controller.getState() < calibrated_min_airspeed) { - _airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed); - } - - if (control_interval > FLT_EPSILON) { - // constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s - _airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval); - } - - if (_airspeed_slew_rate_controller.getState() > _performance_model.getMaximumCalibratedAirspeed()) { - _airspeed_slew_rate_controller.setForcedValue(_performance_model.getMaximumCalibratedAirspeed()); - } - - return _airspeed_slew_rate_controller.getState(); + return manual_airspeed_setpoint; } void -FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, - float true_airspeed_derivative_raw, float throttle_trim) -{ - tecs_status_s tecs_status{}; - - const TECS::DebugOutput &debug_output{_tecs.getStatus()}; - - tecs_status.altitude_sp = alt_sp; - tecs_status.altitude_reference = debug_output.altitude_reference; - tecs_status.altitude_time_constant = _tecs.get_altitude_error_time_constant(); - tecs_status.height_rate_reference = debug_output.height_rate_reference; - tecs_status.height_rate_direct = debug_output.height_rate_direct; - tecs_status.height_rate_setpoint = debug_output.control.altitude_rate_control; - tecs_status.height_rate = -_local_pos.vz; - tecs_status.equivalent_airspeed_sp = equivalent_airspeed_sp; - tecs_status.true_airspeed_sp = debug_output.true_airspeed_sp; - tecs_status.true_airspeed_filtered = debug_output.true_airspeed_filtered; - tecs_status.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control; - tecs_status.true_airspeed_derivative = debug_output.true_airspeed_derivative; - tecs_status.true_airspeed_derivative_raw = true_airspeed_derivative_raw; - tecs_status.total_energy_rate = debug_output.control.total_energy_rate_estimate; - tecs_status.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate; - tecs_status.total_energy_rate_sp = debug_output.control.total_energy_rate_sp; - tecs_status.total_energy_balance_rate_sp = debug_output.control.energy_balance_rate_sp; - tecs_status.throttle_integ = debug_output.control.throttle_integrator; - tecs_status.pitch_integ = debug_output.control.pitch_integrator; - tecs_status.throttle_sp = _tecs.get_throttle_setpoint(); - tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint(); - tecs_status.throttle_trim = throttle_trim; - tecs_status.underspeed_ratio = _tecs.get_underspeed_ratio(); - tecs_status.fast_descend_ratio = debug_output.fast_descend; - - tecs_status.timestamp = hrt_absolute_time(); - - _tecs_status_pub.publish(tecs_status); -} - -void -FixedwingPositionControl::status_publish() -{ - position_controller_status_s pos_ctrl_status = {}; - - npfg_status_s npfg_status = {}; - - npfg_status.wind_est_valid = _wind_valid; - - const float bearing = _npfg.getBearing(); // dont repeat atan2 calc - - pos_ctrl_status.nav_bearing = bearing; - pos_ctrl_status.target_bearing = _target_bearing; - pos_ctrl_status.xtrack_error = _npfg.getTrackError(); - pos_ctrl_status.acceptance_radius = _npfg.switchDistance(500.0f); - - npfg_status.lat_accel = _npfg.getLateralAccel(); - npfg_status.lat_accel_ff = _npfg.getLateralAccelFF(); - npfg_status.heading_ref = _npfg.getHeadingRef(); - npfg_status.bearing = bearing; - npfg_status.bearing_feas = _npfg.getBearingFeas(); - npfg_status.bearing_feas_on_track = _npfg.getOnTrackBearingFeas(); - npfg_status.signed_track_error = _npfg.getTrackError(); - npfg_status.track_error_bound = _npfg.getTrackErrorBound(); - npfg_status.airspeed_ref = _npfg.getAirspeedRef(); - npfg_status.min_ground_speed_ref = _npfg.getMinGroundSpeedRef(); - npfg_status.adapted_period = _npfg.getAdaptedPeriod(); - npfg_status.p_gain = _npfg.getPGain(); - npfg_status.time_const = _npfg.getTimeConst(); - npfg_status.can_run_factor = _npfg.canRun(_local_pos, _wind_valid); - npfg_status.timestamp = hrt_absolute_time(); - - _npfg_status_pub.publish(npfg_status); - - pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); - - pos_ctrl_status.timestamp = hrt_absolute_time(); - - pos_ctrl_status.type = _position_sp_type; - - _pos_ctrl_status_pub.publish(pos_ctrl_status); -} - -void -FixedwingPositionControl::landing_status_publish() +FixedWingModeManager::landing_status_publish() { position_controller_landing_status_s pos_ctrl_landing_status = {}; @@ -509,49 +294,8 @@ FixedwingPositionControl::landing_status_publish() _pos_ctrl_landing_status_pub.publish(pos_ctrl_landing_status); } -float FixedwingPositionControl::getCorrectedNpfgRollSetpoint() -{ - // Scale the npfg output to zero if npfg is not certain for correct output - float new_roll_setpoint(_npfg.getRollSetpoint()); - const float can_run_factor(constrain(_npfg.canRun(_local_pos, _wind_valid), 0.f, 1.f)); - - hrt_abstime now{hrt_absolute_time()}; - - // Warn the user when the scale is less than 90% for at least 2 seconds (disable in transition) - - // If the npfg was not running before, reset the user warning variables. - if ((now - _time_since_last_npfg_call) > ROLL_WARNING_TIMEOUT) { - _need_report_npfg_uncertain_condition = true; - _time_since_first_reduced_roll = 0U; - } - - if (_vehicle_status.in_transition_mode || can_run_factor > ROLL_WARNING_CAN_RUN_THRESHOLD || _landed) { - // NPFG reports a good condition or we are in transition, reset the user warning variables. - _need_report_npfg_uncertain_condition = true; - _time_since_first_reduced_roll = 0U; - - } else if (_need_report_npfg_uncertain_condition) { - if (_time_since_first_reduced_roll == 0U) { - _time_since_first_reduced_roll = now; - } - - if ((now - _time_since_first_reduced_roll) > ROLL_WARNING_TIMEOUT) { - _need_report_npfg_uncertain_condition = false; - events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning, - "Roll command reduced due to uncertain velocity/wind estimates!"); - } - - } else { - // Nothing to do, already reported. - } - - _time_since_last_npfg_call = now; - - return can_run_factor * (new_roll_setpoint); -} - void -FixedwingPositionControl::updateLandingAbortStatus(const uint8_t new_abort_status) +FixedWingModeManager::updateLandingAbortStatus(const uint8_t new_abort_status) { // prevent automatic aborts if already flaring, but allow manual aborts if (!_flare_states.flaring || new_abort_status == position_controller_landing_status_s::ABORTED_BY_OPERATOR) { @@ -591,86 +335,28 @@ FixedwingPositionControl::updateLandingAbortStatus(const uint8_t new_abort_statu } } -void -FixedwingPositionControl::get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev, - position_setpoint_s &waypoint_next, bool flag_init) -{ - position_setpoint_s temp_prev = waypoint_prev; - position_setpoint_s temp_next = waypoint_next; - - if (flag_init) { - // previous waypoint: HDG_HOLD_SET_BACK_DIST meters behind us - waypoint_from_heading_and_distance(_current_latitude, _current_longitude, heading + radians(180.0f), - HDG_HOLD_SET_BACK_DIST, &temp_prev.lat, &temp_prev.lon); - - // next waypoint: HDG_HOLD_DIST_NEXT meters in front of us - waypoint_from_heading_and_distance(_current_latitude, _current_longitude, heading, - HDG_HOLD_DIST_NEXT, &temp_next.lat, &temp_next.lon); - - } else { - // use the existing flight path from prev to next - - // previous waypoint: shifted HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST - create_waypoint_from_line_and_dist(waypoint_next.lat, waypoint_next.lon, waypoint_prev.lat, waypoint_prev.lon, - HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST, &temp_prev.lat, &temp_prev.lon); - - // next waypoint: shifted -(HDG_HOLD_DIST_NEXT + HDG_HOLD_REACHED_DIST) - create_waypoint_from_line_and_dist(waypoint_next.lat, waypoint_next.lon, waypoint_prev.lat, waypoint_prev.lon, - -(HDG_HOLD_REACHED_DIST + HDG_HOLD_DIST_NEXT), &temp_next.lat, &temp_next.lon); - } - - waypoint_prev = temp_prev; - waypoint_prev.alt = _current_altitude; - - waypoint_next = temp_next; - waypoint_next.alt = _current_altitude; -} - float -FixedwingPositionControl::getManualHeightRateSetpoint() +FixedWingModeManager::getManualHeightRateSetpoint() { - /* - * The complete range is -1..+1, so this is 6% - * of the up or down range or 3% of the total range. - */ - const float deadBand = 0.06f; + float height_rate_setpoint = 0.f; - /* - * The correct scaling of the complete range needs - * to account for the missing part of the slope - * due to the deadband - */ - const float factor = 1.0f - deadBand; - - float height_rate_setpoint = 0.0f; - - /* - * Manual control has as convention the rotation around - * an axis. Positive X means to rotate positively around - * the X axis in NED frame, which is pitching down - */ - if (_manual_control_setpoint_for_height_rate > deadBand) { - /* pitching down */ - float pitch = -(_manual_control_setpoint_for_height_rate - deadBand) / factor; - height_rate_setpoint = pitch * _param_sinkrate_target.get(); - - } else if (_manual_control_setpoint_for_height_rate < - deadBand) { - /* pitching up */ - float pitch = -(_manual_control_setpoint_for_height_rate + deadBand) / factor; - const float climb_rate_target = _param_climbrate_target.get(); - - height_rate_setpoint = pitch * climb_rate_target; + if (_manual_control_setpoint_for_height_rate >= FLT_EPSILON) { + height_rate_setpoint = math::interpolate(math::deadzone(_manual_control_setpoint_for_height_rate, + kStickDeadBand), 0, 1.f, 0.f, -_param_sinkrate_target.get()); + } else { + height_rate_setpoint = math::interpolate(math::deadzone(_manual_control_setpoint_for_height_rate, + kStickDeadBand), -1., 0.f, _param_climbrate_target.get(), 0.f); } return height_rate_setpoint; } void -FixedwingPositionControl::updateManualTakeoffStatus() +FixedWingModeManager::updateManualTakeoffStatus() { if (!_completed_manual_takeoff) { - const bool at_controllable_airspeed = _airspeed_eas > _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()) + const bool at_controllable_airspeed = _airspeed_eas > _param_fw_airspd_min.get() || !_airspeed_valid; const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _control_mode.flag_armed; @@ -679,7 +365,7 @@ FixedwingPositionControl::updateManualTakeoffStatus() } void -FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) +FixedWingModeManager::set_control_mode_current(const hrt_abstime &now) { /* only run position controller in fixed-wing mode and during transitions for VTOL */ if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) { @@ -687,7 +373,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) return; // do not publish the setpoint } - FW_POSCTRL_MODE commanded_position_control_mode = _control_mode_current; + const FW_POSCTRL_MODE previous_position_control_mode = _control_mode_current; _skipping_takeoff_detection = false; const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw; @@ -727,7 +413,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) } else { _control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF; - if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) { + if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) { // skip takeoff detection when switching from any other mode, auto or manual, // while already in air. // TODO: find a better place for / way of doing this @@ -758,8 +444,8 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) // failsafe modes engaged if position estimate is invalidated - if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE - && commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE) { + if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE + && previous_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE) { // reset timer the first time we switch into this mode _time_in_fixed_bank_loiter = now; } @@ -770,7 +456,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) } else if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s) && !_vehicle_status.in_transition_mode) { - if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) { + if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) { // Need to init because last loop iteration was in a different mode events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical, "Start loiter with fixed bank angle"); @@ -779,7 +465,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) _control_mode_current = FW_POSCTRL_MODE_AUTO_ALTITUDE; } else { - if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) { + if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) { events::send(events::ID("fixedwing_position_control_descend"), events::Log::Critical, "Start descending"); } @@ -788,19 +474,11 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) } else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) { - if (commanded_position_control_mode != FW_POSCTRL_MODE_MANUAL_POSITION) { + if (previous_position_control_mode != FW_POSCTRL_MODE_MANUAL_POSITION) { /* Need to init because last loop iteration was in a different mode */ _hdg_hold_yaw = _yaw; // yaw is not controlled, so set setpoint to current yaw _hdg_hold_enabled = false; // this makes sure the waypoints are reset below _yaw_lock_engaged = false; - - /* reset setpoints from other modes (auto) otherwise we won't - * level out without new manual input */ - float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - const Eulerf current_setpoint(Quatf(_att_sp.q_d)); - const Quatf setpoint(Eulerf(roll_body, current_setpoint.theta(), yaw_body)); - setpoint.copyTo(_att_sp.q_d); } _control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION; @@ -815,17 +493,16 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) } void -FixedwingPositionControl::update_in_air_states(const hrt_abstime now) +FixedWingModeManager::update_in_air_states(const hrt_abstime now) { /* reset flag when airplane landed */ if (_landed) { _completed_manual_takeoff = false; } - } void -FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_setpoint_s ¤t_sp) +FixedWingModeManager::move_position_setpoint_for_vtol_transition(position_setpoint_s ¤t_sp) { // TODO: velocity, altitude, or just a heading hold position mode should be used for this, not position // shifting hacks @@ -847,7 +524,6 @@ FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_se _transition_waypoint(1) = lon_transition; } - current_sp.lat = _transition_waypoint(0); current_sp.lon = _transition_waypoint(1); @@ -859,9 +535,9 @@ FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_se } void -FixedwingPositionControl::control_auto(const float control_interval, const Vector2d &curr_pos, - const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, - const position_setpoint_s &pos_sp_next) +FixedWingModeManager::control_auto(const float control_interval, const Vector2d &curr_pos, + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, + const position_setpoint_s &pos_sp_next) { position_setpoint_s current_sp = pos_sp_curr; move_position_setpoint_for_vtol_transition(current_sp); @@ -886,13 +562,7 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto switch (position_sp_type) { case position_setpoint_s::SETPOINT_TYPE_IDLE: { - _att_sp.thrust_body[0] = 0.0f; - const float roll_body = 0.0f; - const float pitch_body = radians(_param_fw_psp_off.get()); - const float yaw_body = 0.0f; - - const Quatf setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - setpoint.copyTo(_att_sp.q_d); + control_idle(); break; } @@ -907,12 +577,12 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto case position_setpoint_s::SETPOINT_TYPE_LOITER: #ifdef CONFIG_FIGURE_OF_EIGHT if (current_sp.loiter_pattern == position_setpoint_s::LOITER_TYPE_FIGUREEIGHT) { - controlAutoFigureEight(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp); + controlAutoFigureEight(control_interval, curr_pos, ground_speed, current_sp); } else #endif // CONFIG_FIGURE_OF_EIGHT { - control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next); + control_auto_loiter(control_interval, curr_pos, ground_speed, current_sp, pos_sp_next); } @@ -930,94 +600,93 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto #endif // CONFIG_FIGURE_OF_EIGHT - /* Copy thrust output for publication, handle special cases */ - if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) { - - _att_sp.thrust_body[0] = 0.0f; - - } else { - // when we are landed state we want the motor to spin at idle speed - _att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust(); - } - if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(current_sp); } } +void FixedWingModeManager::control_idle() +{ + const hrt_abstime now = hrt_absolute_time(); + fixed_wing_lateral_setpoint_s lateral_ctrl_sp {empty_lateral_control_setpoint}; + lateral_ctrl_sp.timestamp = now; + lateral_ctrl_sp.lateral_acceleration = 0.0f; + _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); + + fixed_wing_longitudinal_setpoint_s long_contrl_sp {empty_longitudinal_control_setpoint}; + long_contrl_sp.timestamp = now; + long_contrl_sp.pitch_direct = 0.f; + long_contrl_sp.throttle_direct = 0.0f; + _longitudinal_ctrl_sp_pub.publish(long_contrl_sp); + + _ctrl_configuration_handler.setThrottleMax(0.0f); + _ctrl_configuration_handler.setThrottleMin(0.0f); +} + void -FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval) +FixedWingModeManager::control_auto_fixed_bank_alt_hold() { - const bool is_low_height = checkLowHeightConditions(); - - // only control altitude and airspeed ("fixed-bank loiter") - tecs_update_pitch_throttle(control_interval, - _current_altitude, - _performance_model.getCalibratedTrimAirspeed(), - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height); + const hrt_abstime now = hrt_absolute_time(); + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = now, + .altitude = _current_altitude, + .height_rate = NAN, + .equivalent_airspeed = NAN, + .pitch_direct = NAN, + .throttle_direct = NAN + }; - const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle - const float yaw_body = 0.f; + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); + + float throttle_max = _param_fw_thr_max.get(); // Special case: if z or vz estimate is invalid we cannot control height anymore. To prevent a // "climb-away" we set the thrust to MIN in that case. if (_landed || !_local_pos.z_valid || !_local_pos.v_z_valid) { - _att_sp.thrust_body[0] = _param_fw_thr_min.get(); - - } else { - _att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get()); + throttle_max = _param_fw_thr_min.get(); } - const float pitch_body = get_tecs_pitch(); - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + _ctrl_configuration_handler.setThrottleMax(throttle_max); + fixed_wing_lateral_setpoint_s lateral_ctrl_sp = empty_lateral_control_setpoint; + lateral_ctrl_sp.timestamp = hrt_absolute_time(); + const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + lateral_ctrl_sp.lateral_acceleration = rollAngleToLateralAccel(roll_body); + _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); } void -FixedwingPositionControl::control_auto_descend(const float control_interval) +FixedWingModeManager::control_auto_descend() { // Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover, // but not letting it drift too far away. - const float descend_rate = -0.5f; - const bool disable_underspeed_handling = false; - - const bool is_low_height = checkLowHeightConditions(); - - tecs_update_pitch_throttle(control_interval, - _current_altitude, - _performance_model.getCalibratedTrimAirspeed(), - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - descend_rate); + const float descend_rate = 0.5f; - const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle - const float yaw_body = 0.f; + const hrt_abstime now = hrt_absolute_time(); - // Special case: if vz estimate is invalid we cannot control height rate anymore. To prevent a - // "climb-away" we set the thrust to MIN in that case. - _att_sp.thrust_body[0] = (_landed - || !_local_pos.v_z_valid) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = now, + .altitude = NAN, + .height_rate = -descend_rate, + .equivalent_airspeed = NAN, + .pitch_direct = NAN, + .throttle_direct = NAN + }; - const float pitch_body = get_tecs_pitch(); - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); + + _ctrl_configuration_handler.setThrottleMax((_landed + || !_local_pos.v_z_valid) ? _param_fw_thr_min.get() : _param_fw_thr_max.get()); + + fixed_wing_lateral_setpoint_s lateral_ctrl_sp = empty_lateral_control_setpoint; + lateral_ctrl_sp.timestamp = now; + const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + lateral_ctrl_sp.lateral_acceleration = rollAngleToLateralAccel(roll_body); + _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); } uint8_t -FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr, +FixedWingModeManager::handle_setpoint_type(const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next) { uint8_t position_sp_type = pos_sp_curr.type; @@ -1031,13 +700,12 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp /* current waypoint (the one currently heading for) */ curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - const float acc_rad = _npfg.switchDistance(500.0f); + const float acc_rad = _directional_guidance.switchDistance(500.0f); const bool approaching_vtol_backtransition = _vehicle_status.is_vtol && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _position_setpoint_current_valid && pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid; - // check if we should switch to loiter but only if we are not expecting a backtransition to happen if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && !approaching_vtol_backtransition) { @@ -1069,23 +737,11 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp } void -FixedwingPositionControl::control_auto_position(const float control_interval, const Vector2d &curr_pos, +FixedWingModeManager::control_auto_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { - const float acc_rad = _npfg.switchDistance(500.0f); - float tecs_fw_thr_min; - float tecs_fw_thr_max; - - if (pos_sp_curr.gliding_enabled) { - /* enable gliding with this waypoint */ - _tecs.set_speed_weight(2.0f); - tecs_fw_thr_min = 0.0; - tecs_fw_thr_max = 0.0; - - } else { - tecs_fw_thr_min = _param_fw_thr_min.get(); - tecs_fw_thr_max = _param_fw_thr_max.get(); - } + const float acc_rad = _directional_guidance.switchDistance(500.0f); + const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN; // waypoint is a plain navigation waypoint float position_sp_alt = pos_sp_curr.alt; @@ -1122,149 +778,95 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co } } - float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .altitude = position_sp_alt, + .height_rate = NAN, + .equivalent_airspeed = target_airspeed, + .pitch_direct = NAN, + .throttle_direct = NAN + }; + + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); + + float throttle_min = NAN; + float throttle_max = NAN; + + if (pos_sp_curr.gliding_enabled) { + /* enable gliding with this waypoint */ + throttle_min = 0.0; + throttle_max = 0.0; + _ctrl_configuration_handler.setSpeedWeight(2.f); + } + + _ctrl_configuration_handler.setThrottleMax(throttle_max); + _ctrl_configuration_handler.setThrottleMin(throttle_min); Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); + DirectionalGuidanceOutput sp{}; if (_position_setpoint_previous_valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { Vector2f prev_wp_local = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon); - navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + sp = navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } else { - navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + sp = navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } - float roll_body = getCorrectedNpfgRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - - const bool is_low_height = checkLowHeightConditions(); - - tecs_update_pitch_throttle(control_interval, - position_sp_alt, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - tecs_fw_thr_min, - tecs_fw_thr_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height); - - const float pitch_body = get_tecs_pitch(); - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + fixed_wing_lateral_setpoint_s lateral_ctrl_sp{empty_lateral_control_setpoint}; + lateral_ctrl_sp.timestamp = hrt_absolute_time(); + lateral_ctrl_sp.course = sp.course_setpoint; + lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); } void -FixedwingPositionControl::control_auto_velocity(const float control_interval, const Vector2d &curr_pos, +FixedWingModeManager::control_auto_velocity(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { - float tecs_fw_thr_min; - float tecs_fw_thr_max; - - if (pos_sp_curr.gliding_enabled) { - /* enable gliding with this waypoint */ - _tecs.set_speed_weight(2.0f); - tecs_fw_thr_min = 0.0; - tecs_fw_thr_max = 0.0; + //Offboard velocity control + Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy}; + const float target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0))); - } else { - tecs_fw_thr_min = _param_fw_thr_min.get(); - tecs_fw_thr_max = _param_fw_thr_max.get(); - } + const Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + const DirectionalGuidanceOutput sp = navigateBearing(curr_pos_local, target_bearing, ground_speed, _wind_vel); - // waypoint is a plain navigation waypoint - float position_sp_alt = pos_sp_curr.alt; + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); + const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN; - //Offboard velocity control - Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy}; - _target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0))); + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .altitude = pos_sp_curr.alt, + .height_rate = pos_sp_curr.vz, + .equivalent_airspeed = target_airspeed, + .pitch_direct = NAN, + .throttle_direct = NAN + }; - float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel); - float roll_body = getCorrectedNpfgRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - - float yaw_body = _yaw; - const bool disable_underspeed_handling = false; - - const bool is_low_height = checkLowHeightConditions(); - - tecs_update_pitch_throttle(control_interval, - position_sp_alt, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - tecs_fw_thr_min, - tecs_fw_thr_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - pos_sp_curr.vz); - const float pitch_body = get_tecs_pitch(); - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + if (pos_sp_curr.gliding_enabled) { + _ctrl_configuration_handler.setThrottleMin(0.0f); + _ctrl_configuration_handler.setThrottleMax(0.0f); + _ctrl_configuration_handler.setSpeedWeight(2.0f); + } } void -FixedwingPositionControl::control_auto_loiter(const float control_interval, const Vector2d &curr_pos, - const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, +FixedWingModeManager::control_auto_loiter(const float control_interval, const Vector2d &curr_pos, + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next) { + // current waypoint (the one currently heading for) + const Vector2d curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - Vector2d curr_wp{0, 0}; - Vector2d prev_wp{0, 0}; - - /* current waypoint (the one currently heading for) */ - curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - - if (_position_setpoint_previous_valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - prev_wp(0) = pos_sp_prev.lat; - prev_wp(1) = pos_sp_prev.lon; - - } else { - // No valid previous waypoint, go along the line between aircraft and current waypoint - prev_wp = curr_pos; - } - - float airspeed_sp = -1.f; - - if (PX4_ISFINITE(pos_sp_curr.cruising_speed) && - pos_sp_curr.cruising_speed > FLT_EPSILON) { - - airspeed_sp = pos_sp_curr.cruising_speed; - } - - float tecs_fw_thr_min; - float tecs_fw_thr_max; - - if (pos_sp_curr.gliding_enabled) { - /* enable gliding with this waypoint */ - _tecs.set_speed_weight(2.0f); - tecs_fw_thr_min = 0.0; - tecs_fw_thr_max = 0.0; - - } else { - tecs_fw_thr_min = _param_fw_thr_min.get(); - tecs_fw_thr_max = _param_fw_thr_max.get(); - } - - /* waypoint is a loiter waypoint */ float loiter_radius = pos_sp_curr.loiter_radius; if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) { @@ -1275,41 +877,40 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons Vector2f curr_wp_local{_global_local_proj_ref.project(curr_wp(0), curr_wp(1))}; Vector2f vehicle_to_loiter_center{curr_wp_local - curr_pos_local}; - bool is_low_height = checkLowHeightConditions(); + const bool close_to_circle = vehicle_to_loiter_center.norm() < loiter_radius + _directional_guidance.switchDistance( + 500); + + bool enforce_low_height{false}; - const bool close_to_circle = vehicle_to_loiter_center.norm() < loiter_radius + _npfg.switchDistance(500); + float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN; if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid && close_to_circle && _param_fw_lnd_earlycfg.get()) { // We're in a loiter directly before a landing WP. Enable our landing configuration (flaps, // landing airspeed and potentially tighter altitude control) already such that we don't // have to do this switch (which can cause significant altitude errors) close to the ground. + enforce_low_height = true; - // Just before landing, enforce low-height flight conditions for tighter altitude control - is_low_height = true; + if (_param_fw_lnd_airspd.get() > FLT_EPSILON) { + target_airspeed = _param_fw_lnd_airspd.get(); + } - airspeed_sp = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); _new_landing_gear_position = landing_gear_s::GEAR_DOWN; } - float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_sp, - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), - ground_speed); - - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, - ground_speed, - _wind_vel); - float roll_body = getCorrectedNpfgRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + const DirectionalGuidanceOutput sp = navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, + pos_sp_curr.loiter_direction_counter_clockwise, + ground_speed, + _wind_vel); - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; - float alt_sp = pos_sp_curr.alt; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); if (_landing_abort_status) { if (pos_sp_curr.alt - _current_altitude < kClearanceAltitudeBuffer) { @@ -1318,47 +919,47 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons } else { // continue straight until vehicle has sufficient altitude - // keep flaps in landing configuration if the airspeed is below the min airspeed (keep deployed if airspeed not valid) - roll_body = 0.0f; + _ctrl_configuration_handler.setLateralAccelMax(0.0f); - if (!_airspeed_valid || _airspeed_eas < _performance_model.getMinimumCalibratedAirspeed()) { - _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); + // keep flaps in landing configuration if the airspeed is below the min airspeed (keep deployed if airspeed not valid) + if (!_airspeed_valid || _airspeed_eas < _param_fw_airspd_min.get()) { + _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); } else { _flaps_setpoint = 0.f; } } - is_low_height = true; // In low-height flight, TECS will control altitude tighter + enforce_low_height = true; } - tecs_update_pitch_throttle(control_interval, - alt_sp, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - tecs_fw_thr_min, - tecs_fw_thr_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height); + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .altitude = pos_sp_curr.alt, + .height_rate = NAN, + .equivalent_airspeed = target_airspeed, + .pitch_direct = NAN, + .throttle_direct = NAN + }; + + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - const float pitch_body = get_tecs_pitch(); + if (pos_sp_curr.gliding_enabled) { + _ctrl_configuration_handler.setThrottleMin(0.0f); + _ctrl_configuration_handler.setThrottleMax(0.0f); + _ctrl_configuration_handler.setSpeedWeight(2.0f); + } - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + _ctrl_configuration_handler.setEnforceLowHeightCondition(enforce_low_height); } #ifdef CONFIG_FIGURE_OF_EIGHT void -FixedwingPositionControl::controlAutoFigureEight(const float control_interval, const Vector2d &curr_pos, - const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) +FixedWingModeManager::controlAutoFigureEight(const float control_interval, const Vector2d &curr_pos, + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { // airspeed settings - float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, - _performance_model.getMinimumCalibratedAirspeed(), ground_speed); - - // Lateral Control + const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN; Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; @@ -1369,50 +970,36 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c params.loiter_orientation = pos_sp_curr.loiter_orientation; params.loiter_radius = pos_sp_curr.loiter_radius; - // Apply control - _figure_eight.updateSetpoint(curr_pos_local, ground_speed, params, target_airspeed); - float roll_body = _figure_eight.getRollSetpoint(); - target_airspeed = _figure_eight.getAirspeedSetpoint(); - _target_bearing = _figure_eight.getTargetBearing(); - _closest_point_on_path = _figure_eight.getClosestPoint(); - - // TECS - float tecs_fw_thr_min; - float tecs_fw_thr_max; + const DirectionalGuidanceOutput sp = _figure_eight.updateSetpoint(curr_pos_local, ground_speed, params); - if (pos_sp_curr.gliding_enabled) { - /* enable gliding with this waypoint */ - _tecs.set_speed_weight(2.0f); - tecs_fw_thr_min = 0.0; - tecs_fw_thr_max = 0.0; + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; - } else { - tecs_fw_thr_min = _param_fw_thr_min.get(); - tecs_fw_thr_max = _param_fw_thr_max.get(); - } + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - const bool is_low_height = checkLowHeightConditions(); + _closest_point_on_path = _figure_eight.getClosestPoint(); - tecs_update_pitch_throttle(control_interval, - pos_sp_curr.alt, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - tecs_fw_thr_min, - tecs_fw_thr_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height); + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .altitude = pos_sp_curr.alt, + .height_rate = NAN, + .equivalent_airspeed = target_airspeed, + .pitch_direct = NAN, + .throttle_direct = NAN + }; - // Yaw - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - const float pitch_body = get_tecs_pitch(); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + if (pos_sp_curr.gliding_enabled) { + _ctrl_configuration_handler.setThrottleMin(0.0f); + _ctrl_configuration_handler.setThrottleMax(0.0f); + _ctrl_configuration_handler.setSpeedWeight(2.0f); + } } -void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_s pos_sp) +void FixedWingModeManager::publishFigureEightStatus(const position_setpoint_s pos_sp) { figure_eight_status_s figure_eight_status{}; figure_eight_status.timestamp = hrt_absolute_time(); @@ -1429,67 +1016,48 @@ void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_ #endif // CONFIG_FIGURE_OF_EIGHT void -FixedwingPositionControl::control_auto_path(const float control_interval, const Vector2d &curr_pos, - const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) +FixedWingModeManager::control_auto_path(const float control_interval, const Vector2d &curr_pos, + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { - float tecs_fw_thr_min; - float tecs_fw_thr_max; - - if (pos_sp_curr.gliding_enabled) { - /* enable gliding with this waypoint */ - _tecs.set_speed_weight(2.0f); - tecs_fw_thr_min = 0.0; - tecs_fw_thr_max = 0.0; - - } else { - tecs_fw_thr_min = _param_fw_thr_min.get(); - tecs_fw_thr_max = _param_fw_thr_max.get(); - } - - // waypoint is a plain navigation waypoint - float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); + const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN; Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - // Navigate directly on position setpoint and path tangent - matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy); + const matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy); const float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius : 0.0f; - navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature); - - float roll_body = getCorrectedNpfgRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + const DirectionalGuidanceOutput sp = navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), + ground_speed, _wind_vel, curvature); + + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); + + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .altitude = pos_sp_curr.alt, + .height_rate = NAN, + .equivalent_airspeed = target_airspeed, + .pitch_direct = NAN, + .throttle_direct = NAN + }; + + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - - const bool is_low_height = checkLowHeightConditions(); - - tecs_update_pitch_throttle(control_interval, - pos_sp_curr.alt, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - tecs_fw_thr_min, - tecs_fw_thr_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height); - - _att_sp.thrust_body[0] = min(get_tecs_thrust(), tecs_fw_thr_max); - const float pitch_body = get_tecs_pitch(); - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + if (pos_sp_curr.gliding_enabled) { + _ctrl_configuration_handler.setThrottleMin(0.0f); + _ctrl_configuration_handler.setThrottleMax(0.0f); + _ctrl_configuration_handler.setSpeedWeight(2.0f); + } } void -FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float control_interval, +FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &global_position, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { if (!_control_mode.flag_armed) { @@ -1504,27 +1072,18 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo // (navigator will accept the takeoff as complete once crossing the clearance altitude) const float altitude_setpoint_amsl = clearance_altitude_amsl + kClearanceAltitudeBuffer; - Vector2f local_2D_position{_local_pos.x, _local_pos.y}; + const Vector2f local_2D_position{_local_pos.x, _local_pos.y}; const float takeoff_airspeed = (_param_fw_tko_airspd.get() > FLT_EPSILON) ? _param_fw_tko_airspd.get() : - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); - - float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); - - if (takeoff_airspeed < adjusted_min_airspeed) { - // adjust underspeed detection bounds for takeoff airspeed - _tecs.set_equivalent_airspeed_min(takeoff_airspeed); - adjusted_min_airspeed = takeoff_airspeed; - } - - const bool is_low_height = checkLowHeightConditions(); + _param_fw_airspd_min.get(); if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { - _runway_takeoff.init(now, _yaw, global_position); + _runway_takeoff.init(now); + _takeoff_init_position = global_position; _takeoff_ground_alt = _current_altitude; _launch_current_yaw = _yaw; - _airspeed_slew_rate_controller.setForcedValue(takeoff_airspeed); + // _airspeed_slew_rate_controller.setForcedValue(takeoff_airspeed); // TODO events::send(events::ID("fixedwing_position_control_takeoff"), events::Log::Info, "Takeoff on runway"); } @@ -1536,22 +1095,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _runway_takeoff.update(now, takeoff_airspeed, _airspeed_eas, _current_altitude - _takeoff_ground_alt, clearance_altitude_amsl - _takeoff_ground_alt); - // yaw control is disabled once in "taking off" state - _att_sp.fw_control_yaw_wheel = _runway_takeoff.controlYaw(); - - // XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface. - if (_param_rwto_nudge.get()) { - _att_sp.yaw_sp_move_rate = _manual_control_setpoint.yaw; - } - - // tune up the lateral position control guidance when on the ground - if (_runway_takeoff.controlYaw()) { - _npfg.setPeriod(_param_rwto_npfg_period.get()); - - } - - const Vector2f start_pos_local = _global_local_proj_ref.project(_runway_takeoff.getStartPosition()(0), - _runway_takeoff.getStartPosition()(1)); + const Vector2f start_pos_local = _global_local_proj_ref.project(_takeoff_init_position(0), _takeoff_init_position(1)); const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); // by default set the takeoff bearing to the takeoff yaw, but override in a mission takeoff with bearing to takeoff WP @@ -1566,59 +1110,38 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } } - float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed, ground_speed, - true); - - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); - - float roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint()); + const DirectionalGuidanceOutput sp = navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, + _wind_vel); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = now; + fw_lateral_ctrl_sp.course = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; - // use npfg's bearing to commanded course, controlled via yaw angle while on runway - const float bearing = _npfg.getBearing(); + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - // heading hold mode will override this bearing setpoint - float yaw_body = _runway_takeoff.getYaw(bearing); + const float roll_wingtip_strike = getMaxRollAngleNearGround(_current_altitude, _takeoff_ground_alt); + _ctrl_configuration_handler.setLateralAccelMax(rollAngleToLateralAccel(roll_wingtip_strike)); - // update tecs const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get())); const float pitch_min = _runway_takeoff.getMinPitch(math::radians(_takeoff_pitch_min.get()), math::radians(_param_fw_p_lim_min.get())); - if (_runway_takeoff.resetIntegrators()) { - // reset integrals except yaw (which also counts for the wheel controller) - _att_sp.reset_integral = true; - - // throttle is open loop anyway during ground roll, no need to wind up the integrator - _tecs.resetIntegrals(); - } - - const bool disable_underspeed_handling = true; - - tecs_update_pitch_throttle(control_interval, - altitude_setpoint_amsl, - target_airspeed, - pitch_min, - pitch_max, - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - _param_sinkrate_target.get(), - _performance_model.getMaximumClimbRate(_air_density), - is_low_height, - disable_underspeed_handling); - - _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - - const float pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); - _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust()); + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = now, + .altitude = altitude_setpoint_amsl, + .height_rate = NAN, + .equivalent_airspeed = takeoff_airspeed, + .pitch_direct = _runway_takeoff.getPitch(), + .throttle_direct = _runway_takeoff.getThrottle(_param_fw_thr_idle.get()) + }; - roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + _ctrl_configuration_handler.setPitchMin(pitch_min); + _ctrl_configuration_handler.setPitchMax(pitch_max); + _ctrl_configuration_handler.setClimbRateTarget(_param_fw_t_clmb_max.get()); + _ctrl_configuration_handler.setDisableUnderspeedProtection(true); _flaps_setpoint = _param_fw_flaps_to_scl.get(); @@ -1627,6 +1150,13 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _new_landing_gear_position = landing_gear_s::GEAR_UP; } + fixed_wing_runway_control_s fw_runway_control{}; + fw_runway_control.timestamp = now; + fw_runway_control.wheel_steering_enabled = true; + fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _manual_control_setpoint.yaw : 0.f; + + _fixed_wing_runway_control_pub.publish(fw_runway_control); + } else { /* Perform launch detection */ if (!_skipping_takeoff_detection && _param_fw_laun_detcn_on.get() && @@ -1646,14 +1176,14 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo if (!_launch_detected && _launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH) { _launch_detected = true; - _launch_global_position = global_position; + _takeoff_init_position = global_position; _takeoff_ground_alt = _current_altitude; _launch_current_yaw = _yaw; - _airspeed_slew_rate_controller.setForcedValue(takeoff_airspeed); + // _airspeed_slew_rate_controller.setForcedValue(takeoff_airspeed); // TODO } - const Vector2f launch_local_position = _global_local_proj_ref.project(_launch_global_position(0), - _launch_global_position(1)); + const Vector2f launch_local_position = _global_local_proj_ref.project(_takeoff_init_position(0), + _takeoff_init_position(1)); const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); // by default set the takeoff bearing to the takeoff yaw, but override in a mission takeoff with bearing to takeoff WP @@ -1672,61 +1202,52 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo if (_launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH) { /* Launch has been detected, hence we have to control the plane. */ - float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed, ground_speed, - true); + const DirectionalGuidanceOutput sp = navigateLine(launch_local_position, takeoff_bearing, local_2D_position, + ground_speed, + _wind_vel); + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = now; + fw_lateral_ctrl_sp.course = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); - float roll_body = getCorrectedNpfgRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); + const float roll_wingtip_strike = getMaxRollAngleNearGround(_current_altitude, _takeoff_ground_alt); + _ctrl_configuration_handler.setLateralAccelMax(rollAngleToLateralAccel(roll_wingtip_strike)); const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? - _param_fw_thr_idle.get() : _param_fw_thr_max.get(); - const bool disable_underspeed_handling = true; - - tecs_update_pitch_throttle(control_interval, - altitude_setpoint_amsl, - target_airspeed, - radians(_takeoff_pitch_min.get()), - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - max_takeoff_throttle, - _param_sinkrate_target.get(), - _performance_model.getMaximumClimbRate(_air_density), - is_low_height, - disable_underspeed_handling); - - if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) { - // explicitly set idle throttle until motors are enabled - _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); + _param_fw_thr_idle.get() : NAN; + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = now, + .altitude = altitude_setpoint_amsl, + .height_rate = NAN, + .equivalent_airspeed = takeoff_airspeed, + .pitch_direct = NAN, + .throttle_direct = NAN + }; - } else { - _att_sp.thrust_body[0] = get_tecs_thrust(); - } - float pitch_body = get_tecs_pitch(); - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + _ctrl_configuration_handler.setPitchMin(radians(_takeoff_pitch_min.get())); + _ctrl_configuration_handler.setThrottleMax(max_takeoff_throttle); + _ctrl_configuration_handler.setClimbRateTarget(_param_fw_t_clmb_max.get()); + _ctrl_configuration_handler.setDisableUnderspeedProtection(true); - Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + //float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } else { + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = now; + fw_lateral_ctrl_sp.lateral_acceleration = 0.f; /* Tell the attitude controller to stop integrating while we are waiting for the launch */ - _att_sp.reset_integral = true; - - /* Set default roll and pitch setpoints during detection phase */ - float roll_body = 0.0f; - float yaw_body = _yaw; - _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); - float pitch_body = radians(_takeoff_pitch_min.get()); - roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); - Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); + fixed_wing_longitudinal_setpoint_s long_control_sp{empty_longitudinal_control_setpoint}; + long_control_sp.timestamp = now; + long_control_sp.pitch_direct = radians(_takeoff_pitch_min.get()); + long_control_sp.throttle_direct = _param_fw_thr_idle.get(); + _longitudinal_ctrl_sp_pub.publish(long_control_sp); } launch_detection_status_s launch_detection_status; @@ -1743,23 +1264,13 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } void -FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const float control_interval, +FixedWingModeManager::control_auto_landing_straight(const hrt_abstime &now, const float control_interval, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { - // first handle non-position things like airspeed and tecs settings const float airspeed_land = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); - float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); - - if (airspeed_land < adjusted_min_airspeed) { - // adjust underspeed detection bounds for landing airspeed - _tecs.set_equivalent_airspeed_min(airspeed_land); - adjusted_min_airspeed = airspeed_land; - } - - float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed, - ground_speed); + _param_fw_airspd_min.get(); + _ctrl_configuration_handler.setEnforceLowHeightCondition(true); // now handle position const Vector2f local_position{_local_pos.x, _local_pos.y}; @@ -1807,8 +1318,6 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, // flare at the maximum of the altitude determined by the time before touchdown and a minimum flare altitude const float flare_rel_alt = math::max(_param_fw_lnd_fl_time.get() * _local_pos.vz, _param_fw_lnd_flalt.get()); - // During landing, enforce low-height flight conditions for tighter altitude control - const bool is_low_height = true; // the terrain estimate (if enabled) is always used to determine the flaring altitude if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) { @@ -1819,7 +1328,6 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _flare_states.flaring = true; _flare_states.start_time = now; _flare_states.initial_height_rate_setpoint = -_local_pos.vz; - _flare_states.initial_throttle_setpoint = _att_sp.thrust_body[0]; events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info, "Landing, flaring"); } @@ -1830,29 +1338,32 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, 1.0f); /* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */ - - // tune up the lateral position control guidance when on the ground - const float ground_roll_npfg_period = flare_ramp_interpolator * _param_rwto_npfg_period.get() + - (1.0f - flare_ramp_interpolator) * _param_npfg_period.get(); - _npfg.setPeriod(ground_roll_npfg_period); - const Vector2f local_approach_entrance = local_land_point - landing_approach_vector; - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - float roll_body = getCorrectedNpfgRollSetpoint(); + const DirectionalGuidanceOutput sp = navigateLine(local_approach_entrance, local_land_point, local_position, + ground_speed, + _wind_vel); + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = now; + fw_lateral_ctrl_sp.course = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - // use npfg's bearing to commanded course, controlled via yaw angle while on runway - float yaw_body = _npfg.getBearing(); + const float roll_wingtip_strike = getMaxRollAngleNearGround(_current_altitude, _takeoff_ground_alt); + _ctrl_configuration_handler.setLateralAccelMax(rollAngleToLateralAccel(roll_wingtip_strike)); /* longitudinal guidance */ const float flare_ramp_interpolator_sqrt = sqrtf(flare_ramp_interpolator); - const float height_rate_setpoint = flare_ramp_interpolator_sqrt * (-_param_fw_lnd_fl_sink.get()) + - (1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_height_rate_setpoint; + // Use separate ramp for the altitude setpoint that's starting only when the other is finished + // to allow motor to be ramped down before height rate setpoint is adapted for flaring. + const float flare_hieght_rate_interpolator = math::constrain((seconds_since_flare_start - + _param_fw_lnd_fl_time.get()) / (_param_fw_lnd_fl_time.get()), 0.f, 1.f); + const float flare_hieght_rate_interpolator_sqrt = sqrt(flare_hieght_rate_interpolator); + + const float height_rate_setpoint = flare_hieght_rate_interpolator_sqrt * (-_param_fw_lnd_fl_sink.get()) + + (1.0f - flare_hieght_rate_interpolator_sqrt) * _flare_states.initial_height_rate_setpoint; float pitch_min_rad = flare_ramp_interpolator_sqrt * radians(_param_fw_lnd_fl_pmin.get()) + (1.0f - flare_ramp_interpolator_sqrt) * radians(_param_fw_p_lim_min.get()); @@ -1877,47 +1388,23 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); - const bool disable_underspeed_handling = true; - - tecs_update_pitch_throttle(control_interval, - altitude_setpoint, - target_airspeed, - pitch_min_rad, - pitch_max_rad, - _param_fw_thr_idle.get(), - throttle_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - height_rate_setpoint); - - /* set the attitude and throttle commands */ - - // TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle - float pitch_body = get_tecs_pitch(); - - roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = now, + .altitude = altitude_setpoint, + .height_rate = height_rate_setpoint, + .equivalent_airspeed = airspeed_land, + .pitch_direct = NAN, + .throttle_direct = NAN + }; - // enable direct yaw control using rudder/wheel - _att_sp.fw_control_yaw_wheel = true; + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - // XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface. - if (_param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled) { - _att_sp.yaw_sp_move_rate = _manual_control_setpoint.yaw; - } - - // blend the height rate controlled throttle setpoints with initial throttle setting over the flare - // ramp time period to maintain throttle command continuity when switching from altitude to height rate - // control - const float blended_throttle = flare_ramp_interpolator * get_tecs_thrust() + (1.0f - flare_ramp_interpolator) * - _flare_states.initial_throttle_setpoint; - - _att_sp.thrust_body[0] = blended_throttle; + _ctrl_configuration_handler.setPitchMin(pitch_min_rad); + _ctrl_configuration_handler.setPitchMax(pitch_max_rad); + _ctrl_configuration_handler.setThrottleMax(throttle_max); + _ctrl_configuration_handler.setThrottleMin(_param_fw_thr_idle.get()); + _ctrl_configuration_handler.setDisableUnderspeedProtection(true); } else { @@ -1927,50 +1414,48 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const Vector2f local_approach_entrance = local_land_point - landing_approach_vector; - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - float roll_body = getCorrectedNpfgRollSetpoint(); + const DirectionalGuidanceOutput sp = navigateLine(local_approach_entrance, local_land_point, local_position, + ground_speed, + _wind_vel); + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); + + _ctrl_configuration_handler.setLateralAccelMax(rollAngleToLateralAccel(getMaxRollAngleNearGround(_current_altitude, + terrain_alt))); /* longitudinal guidance */ - // open the desired max sink rate to encompass the glide slope if within the aircraft's performance limits + // Open the desired max sink rate to encompass the glide slope. // x/sqrt(x^2+1) = sin(arctan(x)) const float glide_slope_sink_rate = airspeed_land * glide_slope / sqrtf(glide_slope * glide_slope + 1.0f); - const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()), - _param_fw_t_sink_max.get()); - - tecs_update_pitch_throttle(control_interval, - altitude_setpoint, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - desired_max_sinkrate, - _param_climbrate_target.get(), - is_low_height); - - /* set the attitude and throttle commands */ + const float desired_max_sinkrate = math::max(glide_slope_sink_rate, _param_sinkrate_target.get()); - float pitch_body = get_tecs_pitch(); + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .altitude = altitude_setpoint, + .height_rate = NAN, + .equivalent_airspeed = airspeed_land, + .pitch_direct = NAN, + .throttle_direct = NAN + }; - roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - // yaw is not controlled in nominal flight - float yaw_body = _yaw; - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); - - // enable direct yaw control using rudder/wheel - _att_sp.fw_control_yaw_wheel = false; - - _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : get_tecs_thrust(); + _ctrl_configuration_handler.setThrottleMin(_param_fw_thr_idle.get()); + _ctrl_configuration_handler.setThrottleMax(_landed ? _param_fw_thr_idle.get() : NAN); + _ctrl_configuration_handler.setSinkRateTarget(desired_max_sinkrate); } - _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation + fixed_wing_runway_control_s fw_runway_control{}; + fw_runway_control.timestamp = now; + fw_runway_control.wheel_steering_enabled = true; + fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ? + _manual_control_setpoint.yaw : 0.f; + + _fixed_wing_runway_control_pub.publish(fw_runway_control); _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -1986,22 +1471,13 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, } void -FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float control_interval, +FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, const float control_interval, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { - // first handle non-position things like airspeed and tecs settings const float airspeed_land = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); - float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); - - if (airspeed_land < adjusted_min_airspeed) { - // adjust underspeed detection bounds for landing airspeed - _tecs.set_equivalent_airspeed_min(airspeed_land); - adjusted_min_airspeed = airspeed_land; - } + _param_fw_airspd_min.get(); - float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed, - ground_speed); + _ctrl_configuration_handler.setEnforceLowHeightCondition(true); const Vector2f local_position{_local_pos.x, _local_pos.y}; @@ -2026,14 +1502,6 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, loiter_radius = _param_nav_loiter_rad.get(); } - // During landing, enforce low-height flight conditions for tighter altitude control - const bool is_low_height = true; - - // the terrain estimate (if enabled) is always used to determine the flaring altitude - float roll_body; - float yaw_body; - float pitch_body; - if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) { // flare and land with minimal speed @@ -2042,7 +1510,6 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _flare_states.flaring = true; _flare_states.start_time = now; _flare_states.initial_height_rate_setpoint = -_local_pos.vz; - _flare_states.initial_throttle_setpoint = _att_sp.thrust_body[0]; events::send(events::ID("fixedwing_position_control_landing_circle_flaring"), events::Log::Info, "Landing, flaring"); } @@ -2053,23 +1520,15 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, 1.0f); /* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */ - - // tune up the lateral position control guidance when on the ground - const float ground_roll_npfg_period = flare_ramp_interpolator * _param_rwto_npfg_period.get() + - (1.0f - flare_ramp_interpolator) * _param_npfg_period.get(); - - _npfg.setPeriod(ground_roll_npfg_period); - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - - navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, - pos_sp_curr.loiter_direction_counter_clockwise, - ground_speed, _wind_vel); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - roll_body = getCorrectedNpfgRollSetpoint(); - - yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - + const DirectionalGuidanceOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, + pos_sp_curr.loiter_direction_counter_clockwise, + ground_speed, _wind_vel); + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = now; + fw_lateral_ctrl_sp.course = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; + + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); /* longitudinal guidance */ const float flare_ramp_interpolator_sqrt = sqrtf(flare_ramp_interpolator); @@ -2099,99 +1558,70 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); - const bool disable_underspeed_handling = true; - - tecs_update_pitch_throttle(control_interval, - _current_altitude, // is not controlled, control descend rate - target_airspeed, - pitch_min_rad, - pitch_max_rad, - _param_fw_thr_idle.get(), - throttle_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - height_rate_setpoint); - - /* set the attitude and throttle commands */ - - // TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle - pitch_body = get_tecs_pitch(); - - // enable direct yaw control using rudder/wheel - _att_sp.fw_control_yaw_wheel = true; - - // XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface. - if (_param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled) { - _att_sp.yaw_sp_move_rate = _manual_control_setpoint.yaw; - } - - // blend the height rate controlled throttle setpoints with initial throttle setting over the flare - // ramp time period to maintain throttle command continuity when switching from altitude to height rate - // control - const float blended_throttle = flare_ramp_interpolator * get_tecs_thrust() + (1.0f - flare_ramp_interpolator) * - _flare_states.initial_throttle_setpoint; - - _att_sp.thrust_body[0] = blended_throttle; + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = now, + .altitude = NAN, + .height_rate = height_rate_setpoint, + .equivalent_airspeed = airspeed_land, + .pitch_direct = NAN, + .throttle_direct = NAN + }; + + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); + + _ctrl_configuration_handler.setPitchMin(pitch_min_rad); + _ctrl_configuration_handler.setPitchMax(pitch_max_rad); + _ctrl_configuration_handler.setThrottleMax(throttle_max); + _ctrl_configuration_handler.setThrottleMin(_param_fw_thr_idle.get()); + _ctrl_configuration_handler.setDisableUnderspeedProtection(true); } else { // follow the glide slope + const DirectionalGuidanceOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, + pos_sp_curr.loiter_direction_counter_clockwise, + ground_speed, _wind_vel); + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = now; + fw_lateral_ctrl_sp.course = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; - /* lateral guidance */ - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - - navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, - pos_sp_curr.loiter_direction_counter_clockwise, - ground_speed, _wind_vel); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - roll_body = getCorrectedNpfgRollSetpoint(); + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); /* longitudinal guidance */ - // open the desired max sink rate to encompass the glide slope if within the aircraft's performance limits + // Open the desired max sink rate to encompass the glide slope. // x/sqrt(x^2+1) = sin(arctan(x)) const float glide_slope = math::radians(_param_fw_lnd_ang.get()); const float glide_slope_sink_rate = airspeed_land * glide_slope / sqrtf(glide_slope * glide_slope + 1.0f); - const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()), - _param_fw_t_sink_max.get()); - const bool disable_underspeed_handling = false; - - tecs_update_pitch_throttle(control_interval, - _current_altitude, // is not controlled, control descend rate - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - desired_max_sinkrate, - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - -glide_slope_sink_rate); // heightrate = -sinkrate - - /* set the attitude and throttle commands */ + const float desired_max_sinkrate = math::max(glide_slope_sink_rate, _param_sinkrate_target.get()); - pitch_body = get_tecs_pitch(); + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = now, + .altitude = NAN, + .height_rate = -glide_slope_sink_rate, + .equivalent_airspeed = airspeed_land, + .pitch_direct = NAN, + .throttle_direct = NAN + }; - // yaw is not controlled in nominal flight - yaw_body = _yaw; + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - // enable direct yaw control using rudder/wheel - _att_sp.fw_control_yaw_wheel = false; - - _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : get_tecs_thrust(); + _ctrl_configuration_handler.setThrottleMin(_param_fw_thr_idle.get()); + _ctrl_configuration_handler.setThrottleMax(_landed ? _param_fw_thr_idle.get() : NAN); + _ctrl_configuration_handler.setSinkRateTarget(desired_max_sinkrate); } - _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - - roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); + fixed_wing_runway_control_s fw_runway_control{}; + fw_runway_control.timestamp = now; + fw_runway_control.wheel_steering_enabled = true; + fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ? + _manual_control_setpoint.yaw : 0.f; - Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + _fixed_wing_runway_control_pub.publish(fw_runway_control); + _ctrl_configuration_handler.setLateralAccelMax(rollAngleToLateralAccel(getMaxRollAngleNearGround(_current_altitude, + terrain_alt))); _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -2205,71 +1635,63 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, } void -FixedwingPositionControl::control_manual_altitude(const float control_interval, const Vector2d &curr_pos, +FixedWingModeManager::control_manual_altitude(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed) { updateManualTakeoffStatus(); - const float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(), - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed, !_completed_manual_takeoff); const float height_rate_sp = getManualHeightRateSetpoint(); // TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are // just passed launch - const float min_pitch = (_completed_manual_takeoff) ? radians(_param_fw_p_lim_min.get()) : + const float min_pitch = (_completed_manual_takeoff) ? NAN : MIN_PITCH_DURING_MANUAL_TAKEOFF; - float throttle_max = _param_fw_thr_max.get(); + float throttle_max = NAN; // enable the operator to kill the throttle on ground if (_landed && (_manual_control_setpoint_for_airspeed < THROTTLE_THRESH)) { throttle_max = 0.0f; } + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .altitude = NAN, + .height_rate = height_rate_sp, + .equivalent_airspeed = get_manual_airspeed_setpoint(), + .pitch_direct = NAN, + .throttle_direct = NAN + }; - const bool is_low_height = checkLowHeightConditions(); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - const bool disable_underspeed_handling = false; + _ctrl_configuration_handler.setPitchMin(min_pitch); + _ctrl_configuration_handler.setThrottleMax(throttle_max); - tecs_update_pitch_throttle(control_interval, - _current_altitude, - calibrated_airspeed_sp, - min_pitch, - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - throttle_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - height_rate_sp); - - float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - - _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); - const float pitch_body = get_tecs_pitch(); - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + const DirectionalGuidanceOutput sp = {.course_setpoint = NAN, .lateral_acceleration_feedforward = rollAngleToLateralAccel(roll_body)}; + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); } void -FixedwingPositionControl::control_manual_position(const float control_interval, const Vector2d &curr_pos, +FixedWingModeManager::control_manual_position(const hrt_abstime now, const float control_interval, + const Vector2d &curr_pos, const Vector2f &ground_speed) { updateManualTakeoffStatus(); - float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(), - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed, !_completed_manual_takeoff); const float height_rate_sp = getManualHeightRateSetpoint(); // TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are // just passed launch - const float min_pitch = (_completed_manual_takeoff) ? radians(_param_fw_p_lim_min.get()) : + const float min_pitch = (_completed_manual_takeoff) ? NAN : MIN_PITCH_DURING_MANUAL_TAKEOFF; - float throttle_max = _param_fw_thr_max.get(); + float throttle_max = NAN; // enable the operator to kill the throttle on ground if (_landed && (_manual_control_setpoint_for_airspeed < THROTTLE_THRESH)) { @@ -2277,14 +1699,9 @@ FixedwingPositionControl::control_manual_position(const float control_interval, } if (_local_pos.xy_reset_counter != _xy_reset_counter) { - _time_last_xy_reset = _local_pos.timestamp; + _time_last_xy_reset = now; } - Eulerf current_setpoint(Quatf(_att_sp.q_d)); - float yaw_body = current_setpoint.psi(); - float roll_body = current_setpoint.phi(); - float pitch_body = current_setpoint.theta(); - /* heading control */ // TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here) if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH && @@ -2306,7 +1723,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, if (_yaw_lock_engaged) { - Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + const Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; if (!_hdg_hold_enabled) { // just switched back from non heading-hold to heading hold @@ -2318,37 +1735,34 @@ FixedwingPositionControl::control_manual_position(const float control_interval, // if there's a reset-by-fusion, the ekf needs some time to converge, // therefore we go into track holiding for 2 seconds - if (_local_pos.timestamp - _time_last_xy_reset < 2_s) { + if (now - _time_last_xy_reset < 2_s) { _hdg_hold_position = curr_pos_local; } - _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(_hdg_hold_position, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); - roll_body = getCorrectedNpfgRollSetpoint(); - calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; + const DirectionalGuidanceOutput sp = navigateLine(_hdg_hold_position, _hdg_hold_yaw, curr_pos_local, ground_speed, + _wind_vel); + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = now; + fw_lateral_ctrl_sp.course = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; - yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); } } - const bool is_low_height = checkLowHeightConditions(); - - const bool disable_underspeed_handling = false; + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .altitude = NAN, + .height_rate = height_rate_sp, + .equivalent_airspeed = get_manual_airspeed_setpoint(), + .pitch_direct = NAN, + .throttle_direct = NAN + }; + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - tecs_update_pitch_throttle(control_interval, - _current_altitude, // TODO: check if this is really what we want.. or if we want to lock the altitude. - calibrated_airspeed_sp, - min_pitch, - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - throttle_max, - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height, - disable_underspeed_handling, - height_rate_sp); + _ctrl_configuration_handler.setPitchMin(min_pitch); + _ctrl_configuration_handler.setThrottleMax(throttle_max); if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.roll) >= HDG_HOLD_MAN_INPUT_THRESH || fabsf(_manual_control_setpoint.yaw) >= HDG_HOLD_MAN_INPUT_THRESH) { @@ -2356,105 +1770,58 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _hdg_hold_enabled = false; _yaw_lock_engaged = false; - roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.lateral_acceleration = rollAngleToLateralAccel(roll_body); + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); } +} - _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); - - pitch_body = get_tecs_pitch(); - - Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); +float FixedWingModeManager::rollAngleToLateralAccel(float roll_body) const +{ + return tanf(roll_body) * CONSTANTS_ONE_G; } -void FixedwingPositionControl::control_backtransition_heading_hold() +void FixedWingModeManager::control_backtransition_heading_hold() { if (!PX4_ISFINITE(_backtrans_heading)) { _backtrans_heading = _local_pos.heading; } - float true_airspeed = _airspeed_eas * _eas2tas; - - if (!_airspeed_valid) { - true_airspeed = _performance_model.getCalibratedTrimAirspeed() * _eas2tas; - } - - // we can achieve heading control by setting airspeed and groundspeed vector equal - const Vector2f airspeed_vector = Vector2f(cosf(_local_pos.heading), sinf(_local_pos.heading)) * true_airspeed; - const Vector2f &ground_speed = airspeed_vector; - - _npfg.setAirspeedNom(_performance_model.getCalibratedTrimAirspeed() * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - - Vector2f virtual_target_point = Vector2f(cosf(_backtrans_heading), sinf(_backtrans_heading)) * HDG_HOLD_DIST_NEXT; - - navigateLine(Vector2f(0.f, 0.f), virtual_target_point, Vector2f(0.f, 0.f), ground_speed, Vector2f(0.f, 0.f)); - - const float roll_body = getCorrectedNpfgRollSetpoint(); - - const float yaw_body = _backtrans_heading; - - // these values are overriden by transition logic - _att_sp.thrust_body[0] = _param_fw_thr_min.get(); - const float pitch_body = 0.0f; - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); - + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.airspeed_direction = _backtrans_heading; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); } -void FixedwingPositionControl::control_backtransition_line_follow(const Vector2f &ground_speed, +void FixedWingModeManager::control_backtransition_line_follow(const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); - _npfg.setAirspeedNom(_performance_model.getCalibratedTrimAirspeed() * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - // Set the position where the backtransition started the first ime we pass through here. // Will get reset if not in transition anymore. if (!_lpos_where_backtrans_started.isAllFinite()) { _lpos_where_backtrans_started = curr_pos_local; } - navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); - const float roll_body = getCorrectedNpfgRollSetpoint(); - - const float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - - // these values are overriden by transition logic - _att_sp.thrust_body[0] = _param_fw_thr_min.get(); - const float pitch_body = 0.0f; - - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); -} -float -FixedwingPositionControl::get_tecs_pitch() -{ - if (_tecs_is_running) { - return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get()); - } - - // return level flight pitch offset to prevent stale tecs state when it's not running - return radians(_param_fw_psp_off.get()); -} + DirectionalGuidanceOutput sp = {.course_setpoint = NAN, .lateral_acceleration_feedforward = 0.f}; -float -FixedwingPositionControl::get_tecs_thrust() -{ - if (_tecs_is_running) { - return min(_tecs.get_throttle_setpoint(), 1.f); + if (_control_mode.flag_control_position_enabled) { + sp = navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } - // return 0 to prevent stale tecs state when it's not running - return 0.0f; + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = hrt_absolute_time(); + fw_lateral_ctrl_sp.course = sp.course_setpoint; + fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); } void -FixedwingPositionControl::Run() +FixedWingModeManager::Run() { if (should_exit()) { _local_pos_sub.unregisterCallback(); @@ -2464,13 +1831,30 @@ FixedwingPositionControl::Run() perf_begin(_loop_perf); - /* only run controller if position changed */ + if (_vehicle_status_sub.updated()) { + + if (_vehicle_status_sub.update(&_vehicle_status)) { + _nav_state = _vehicle_status.nav_state; + } + } + + /* only run controller if position changed and we are not running an external mode*/ + + const bool is_external_nav_state = (_nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1) + && (_nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8); - if (_local_pos_sub.update(&_local_pos)) { + if (is_external_nav_state) { + // this will cause the configuration handler to publish immediately the next time an internal flight + // mode is active + _ctrl_configuration_handler.resetLastPublishTime(); - const float control_interval = math::constrain((_local_pos.timestamp - _last_time_position_control_called) * 1e-6f, + } else if (_local_pos_sub.update(&_local_pos)) { + + const hrt_abstime now = _local_pos.timestamp; + + const float control_interval = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP); - _last_time_position_control_called = _local_pos.timestamp; + _last_time_position_control_called = now; // check for parameter updates if (_parameter_update_sub.updated()) { @@ -2500,11 +1884,6 @@ FixedwingPositionControl::Run() // handle estimator reset events. we only adjust setpoins for manual modes if (_control_mode.flag_control_manual_enabled) { - if (_control_mode.flag_control_altitude_enabled && _local_pos.z_reset_counter != _z_reset_counter) { - // make TECS accept step in altitude and demanded altitude - _tecs.handle_alt_step(_current_altitude, -_local_pos.vz); - } - // adjust navigation waypoints in position control mode if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled && _local_pos.xy_reset_counter != _xy_reset_counter) { @@ -2612,15 +1991,7 @@ FixedwingPositionControl::Run() vehicle_attitude_poll(); vehicle_command_poll(); vehicle_control_mode_poll(); - wind_poll(); - - vehicle_air_data_s air_data; - - if (_vehicle_air_data_sub.update(&air_data)) { - _air_density = PX4_ISFINITE(air_data.rho) ? air_data.rho : _air_density; - _tecs.set_max_climb_rate(_performance_model.getMaximumClimbRate(_air_density)); - _tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density)); - } + wind_poll(now); if (_vehicle_land_detected_sub.updated()) { vehicle_land_detected_s vehicle_land_detected; @@ -2630,41 +2001,31 @@ FixedwingPositionControl::Run() } } - if (_vehicle_status_sub.update(&_vehicle_status)) { - if (!_vehicle_status.in_transition_mode) { - // reset position of backtransition start if not in transition - _lpos_where_backtrans_started = Vector2f(NAN, NAN); - _backtrans_heading = NAN; - } + if (!_vehicle_status.in_transition_mode) { + // reset position of backtransition start if not in transition + _lpos_where_backtrans_started = Vector2f(NAN, NAN); + _backtrans_heading = NAN; } + Vector2d curr_pos(_current_latitude, _current_longitude); Vector2f ground_speed(_local_pos.vx, _local_pos.vy); - set_control_mode_current(_local_pos.timestamp); + set_control_mode_current(now); - update_in_air_states(_local_pos.timestamp); + update_in_air_states(now); // restore nominal TECS parameters in case changed intermittently (e.g. in landing handling) - _tecs.set_speed_weight(_param_fw_t_spdweight.get()); // restore lateral-directional guidance parameters (changed in takeoff mode) - _npfg.setPeriod(_param_npfg_period.get()); - - _att_sp.reset_integral = false; + _directional_guidance.setPeriod(_param_npfg_period.get()); // by default no flaps/spoilers, is overwritten below in certain modes _flaps_setpoint = 0.f; _spoilers_setpoint = 0.f; - // reset flight phase estimate - _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN; - - // by default we don't want yaw to be contoller directly with rudder - _att_sp.fw_control_yaw_wheel = false; - - // default to zero - is used (IN A HACKY WAY) to pass direct nose wheel steering via yaw stick to the actuators during auto takeoff - _att_sp.yaw_sp_move_rate = 0.0f; + // by default set speed weight to the param value, can be overwritten inside the methods below + _ctrl_configuration_handler.setSpeedWeight(_param_t_spdweight.get()); if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT && _control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR) { @@ -2686,23 +2047,23 @@ FixedwingPositionControl::Run() } case FW_POSCTRL_MODE_AUTO_ALTITUDE: { - control_auto_fixed_bank_alt_hold(control_interval); + control_auto_fixed_bank_alt_hold(); break; } case FW_POSCTRL_MODE_AUTO_CLIMBRATE: { - control_auto_descend(control_interval); + control_auto_descend(); break; } case FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT: { - control_auto_landing_straight(_local_pos.timestamp, control_interval, ground_speed, _pos_sp_triplet.previous, + control_auto_landing_straight(now, control_interval, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current); break; } case FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR: { - control_auto_landing_circular(_local_pos.timestamp, control_interval, ground_speed, _pos_sp_triplet.current); + control_auto_landing_circular(now, control_interval, ground_speed, _pos_sp_triplet.current); break; } @@ -2712,12 +2073,12 @@ FixedwingPositionControl::Run() } case FW_POSCTRL_MODE_AUTO_TAKEOFF: { - control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current); + control_auto_takeoff(now, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current); break; } case FW_POSCTRL_MODE_MANUAL_POSITION: { - control_manual_position(control_interval, curr_pos, ground_speed); + control_manual_position(now, control_interval, curr_pos, ground_speed); break; } @@ -2727,7 +2088,6 @@ FixedwingPositionControl::Run() } case FW_POSCTRL_MODE_OTHER: { - _att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get()); break; } @@ -2742,52 +2102,16 @@ FixedwingPositionControl::Run() } } - if (_control_mode_current != FW_POSCTRL_MODE_OTHER) { - Eulerf attitude_setpoint(Quatf(_att_sp.q_d)); - float roll_body = attitude_setpoint.phi(); - float pitch_body = attitude_setpoint.theta(); - float yaw_body = attitude_setpoint.psi(); - - if (_control_mode.flag_control_manual_enabled) { - roll_body = constrain(roll_body, -radians(_param_fw_r_lim.get()), - radians(_param_fw_r_lim.get())); - pitch_body = constrain(pitch_body, radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get())); - } - - if (_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_acceleration_enabled || - _control_mode.flag_control_altitude_enabled || - _control_mode.flag_control_climb_rate_enabled) { - - // roll slew rate - roll_body = _roll_slew_rate.update(roll_body, control_interval); - - const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); - q.copyTo(_att_sp.q_d); - - _att_sp.timestamp = hrt_absolute_time(); - _attitude_sp_pub.publish(_att_sp); - - // only publish status in full FW mode - if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING - || _vehicle_status.in_transition_mode) { - status_publish(); - - } - } - - } else { - _roll_slew_rate.setForcedValue(_roll); + _ctrl_configuration_handler.update(now); } + // only publish status in full FW mode + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + || _vehicle_status.in_transition_mode) { + publish_lateral_guidance_status(now); - - // Publish estimate of level flight - _flight_phase_estimation_pub.get().timestamp = hrt_absolute_time(); - _flight_phase_estimation_pub.update(); + } // if there's any change in landing gear setpoint publish it if (_new_landing_gear_position != old_landing_gear_position @@ -2795,7 +2119,7 @@ FixedwingPositionControl::Run() landing_gear_s landing_gear = {}; landing_gear.landing_gear = _new_landing_gear_position; - landing_gear.timestamp = hrt_absolute_time(); + landing_gear.timestamp = now; _landing_gear_pub.publish(landing_gear); } @@ -2804,16 +2128,15 @@ FixedwingPositionControl::Run() && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { normalized_unsigned_setpoint_s flaps_setpoint; flaps_setpoint.normalized_setpoint = _flaps_setpoint; - flaps_setpoint.timestamp = hrt_absolute_time(); + flaps_setpoint.timestamp = now; _flaps_setpoint_pub.publish(flaps_setpoint); normalized_unsigned_setpoint_s spoilers_setpoint; spoilers_setpoint.normalized_setpoint = _spoilers_setpoint; - spoilers_setpoint.timestamp = hrt_absolute_time(); + spoilers_setpoint.timestamp = now; _spoilers_setpoint_pub.publish(spoilers_setpoint); } - _z_reset_counter = _local_pos.z_reset_counter; _xy_reset_counter = _local_pos.xy_reset_counter; perf_end(_loop_perf); @@ -2821,7 +2144,7 @@ FixedwingPositionControl::Run() } void -FixedwingPositionControl::reset_takeoff_state() +FixedWingModeManager::reset_takeoff_state() { _runway_takeoff.reset(); @@ -2833,7 +2156,7 @@ FixedwingPositionControl::reset_takeoff_state() } void -FixedwingPositionControl::reset_landing_state() +FixedWingModeManager::reset_landing_state() { _time_started_landing = 0; @@ -2844,86 +2167,14 @@ FixedwingPositionControl::reset_landing_state() _last_time_terrain_alt_was_valid = 0; // reset abort land, unless loitering after an abort - if (_landing_abort_status && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER)) { + if ((_landing_abort_status && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER)) || + (_landing_abort_status && _param_fw_lnd_abort.get() == 0)) { updateLandingAbortStatus(position_controller_landing_status_s::NOT_ABORTED); } } -void -FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, - float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, - const float desired_max_sinkrate, const float desired_max_climbrate, const bool is_low_height, - bool disable_underspeed_detection, float hgt_rate_sp) -{ - // do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition - if (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - || _vehicle_status.in_transition_mode)) { - _tecs_is_running = false; - return; - - } else { - _tecs_is_running = true; - } - - /* update TECS vehicle state estimates */ - const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min, - throttle_max, airspeed_sp, _air_density); - - /* No underspeed protection in landing mode */ - _tecs.set_detect_underspeed_enabled(!disable_underspeed_detection); - - updateTECSAltitudeTimeConstant(is_low_height, control_interval); - - // HOTFIX: the airspeed rate estimate using acceleration in body-forward direction has shown to lead to high biases - // when flying tight turns. It's in this case much safer to just set the estimated airspeed rate to 0. - const float airspeed_rate_estimate = 0.f; - - _tecs.update(_pitch - radians(_param_fw_psp_off.get()), - _current_altitude, - alt_sp, - airspeed_sp, - _airspeed_eas, - _eas2tas, - throttle_min, - throttle_max, - throttle_trim_compensated, - pitch_min_rad - radians(_param_fw_psp_off.get()), - pitch_max_rad - radians(_param_fw_psp_off.get()), - desired_max_climbrate, - desired_max_sinkrate, - airspeed_rate_estimate, - -_local_pos.vz, - hgt_rate_sp); - - tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated); - - if (_tecs_is_running && !_vehicle_status.in_transition_mode - && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { - const TECS::DebugOutput &tecs_output{_tecs.getStatus()}; - - // Check level flight: the height rate setpoint is not set or set to 0 and we are close to the target altitude and target altitude is not moving - if ((fabsf(tecs_output.height_rate_reference) < MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT) && - fabsf(_current_altitude - tecs_output.altitude_reference) < _param_nav_fw_alt_rad.get()) { - _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_LEVEL; - - } else if (((tecs_output.altitude_reference - _current_altitude) >= _param_nav_fw_alt_rad.get()) || - (tecs_output.height_rate_reference >= MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) { - _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_CLIMB; - - } else if (((_current_altitude - tecs_output.altitude_reference) >= _param_nav_fw_alt_rad.get()) || - (tecs_output.height_rate_reference <= -MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) { - _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND; - - } else { - //We can't infer the flight phase , do nothing, estimation is reset at each step - } - } -} - -float -FixedwingPositionControl::constrainRollNearGround(const float roll_setpoint, const float altitude, - const float terrain_altitude) const +float FixedWingModeManager::getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const { // we want the wings level when at the wing height above ground const float height_above_ground = math::max(altitude - (terrain_altitude + _param_fw_wing_height.get()), 0.0f); @@ -2933,13 +2184,14 @@ FixedwingPositionControl::constrainRollNearGround(const float roll_setpoint, con // d(roll strike)/d(height) = 2 / span / cos(2 * height / span) // d(roll strike)/d(height) (@height=0) = 2 / span // roll strike ~= 2 * height / span - const float roll_wingtip_strike = 2.0f * height_above_ground / _param_fw_wing_span.get(); - return math::constrain(roll_setpoint, -roll_wingtip_strike, roll_wingtip_strike); + return math::constrain(2.f * height_above_ground / _param_fw_wing_span.get(), 0.f, + math::radians(_param_fw_r_lim.get())); } + void -FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev, +FixedWingModeManager::initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev, const float land_point_altitude, const Vector2f &local_position, const Vector2f &local_land_point) { if (_time_started_landing == 0) { @@ -2997,29 +2249,8 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po } } -bool FixedwingPositionControl::checkLowHeightConditions() -{ - // Are conditions for low-height - return _param_fw_t_thr_low_hgt.get() >= 0.f && _local_pos.dist_bottom_valid - && _local_pos.dist_bottom < _param_fw_t_thr_low_hgt.get(); -} - -void FixedwingPositionControl::updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt) -{ - // Target time constant for the TECS altitude tracker - float alt_tracking_tc = _param_fw_t_h_error_tc.get(); - - if (is_low_height) { - // If low-height conditions satisfied, compute target time constant for altitude tracking - alt_tracking_tc *= _param_fw_thrtc_sc.get(); - } - - _tecs_alt_time_const_slew_rate.update(alt_tracking_tc, dt); - _tecs.set_altitude_error_time_constant(_tecs_alt_time_const_slew_rate.getState()); -} - Vector2f -FixedwingPositionControl::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position) +FixedWingModeManager::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position) { if (fabsf(_manual_control_setpoint.yaw) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE && _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled @@ -3030,8 +2261,8 @@ FixedwingPositionControl::calculateTouchdownPosition(const float control_interva _manual_control_setpoint.yaw); _lateral_touchdown_position_offset += (_manual_control_setpoint.yaw - signed_deadzone_threshold) * MAX_TOUCHDOWN_POSITION_NUDGE_RATE * control_interval; - _lateral_touchdown_position_offset = math::constrain(_lateral_touchdown_position_offset, -_param_fw_lnd_td_off.get(), - _param_fw_lnd_td_off.get()); + _lateral_touchdown_position_offset = math::constrain(_lateral_touchdown_position_offset, -_param_fw_lnd_td_off.get(), + _param_fw_lnd_td_off.get()); } const Vector2f approach_unit_vector = -_landing_approach_entrance_offset_vector.unit_or_zero(); @@ -3041,7 +2272,7 @@ FixedwingPositionControl::calculateTouchdownPosition(const float control_interva } Vector2f -FixedwingPositionControl::calculateLandingApproachVector() const +FixedWingModeManager::calculateLandingApproachVector() const { Vector2f landing_approach_vector = -_landing_approach_entrance_offset_vector; const Vector2f approach_unit_vector = landing_approach_vector.unit_or_zero(); @@ -3063,7 +2294,7 @@ FixedwingPositionControl::calculateLandingApproachVector() const } float -FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude, +FixedWingModeManager::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude, const bool abort_on_terrain_measurement_timeout, const bool abort_on_terrain_timeout) { if (_param_fw_lnd_useter.get() > TerrainEstimateUseOnLanding::kDisableTerrainEstimation) { @@ -3103,7 +2334,7 @@ FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &n return land_point_altitude; } -bool FixedwingPositionControl::checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask, +bool FixedWingModeManager::checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask, uint8_t landing_abort_criterion) { // landing abort status contains a manual criterion at abort_status==1, need to subtract 2 to directly compare @@ -3117,7 +2348,7 @@ bool FixedwingPositionControl::checkLandingAbortBitMask(const uint8_t automatic_ return ((1 << landing_abort_criterion) & automatic_abort_criteria_bitmask) == (1 << landing_abort_criterion); } -void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint) +void FixedWingModeManager::publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint) { vehicle_local_position_setpoint_s local_position_setpoint{}; local_position_setpoint.timestamp = hrt_absolute_time(); @@ -3137,13 +2368,10 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo local_position_setpoint.acceleration[0] = NAN; local_position_setpoint.acceleration[1] = NAN; local_position_setpoint.acceleration[2] = NAN; - local_position_setpoint.thrust[0] = _att_sp.thrust_body[0]; - local_position_setpoint.thrust[1] = _att_sp.thrust_body[1]; - local_position_setpoint.thrust[2] = _att_sp.thrust_body[2]; _local_pos_sp_pub.publish(local_position_setpoint); } -void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp) +void FixedWingModeManager::publishOrbitStatus(const position_setpoint_s pos_sp) { orbit_status_s orbit_status{}; orbit_status.timestamp = hrt_absolute_time(); @@ -3162,7 +2390,8 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_ _orbit_status_pub.publish(orbit_status); } -void FixedwingPositionControl::navigateWaypoints(const Vector2f &start_waypoint, const Vector2f &end_waypoint, +DirectionalGuidanceOutput FixedWingModeManager::navigateWaypoints(const Vector2f &start_waypoint, + const Vector2f &end_waypoint, const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) { const Vector2f start_waypoint_to_end_waypoint = end_waypoint - start_waypoint; @@ -3172,15 +2401,13 @@ void FixedwingPositionControl::navigateWaypoints(const Vector2f &start_waypoint, if (start_waypoint_to_end_waypoint.norm() < FLT_EPSILON) { // degenerate case: the waypoints are on top of each other, this should only happen when someone uses this // method incorrectly. just as a safe guard, call the singular waypoint navigation method. - navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); - return; + return navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); } if ((start_waypoint_to_end_waypoint.dot(start_waypoint_to_vehicle) < -FLT_EPSILON) - && (start_waypoint_to_vehicle.norm() > _npfg.switchDistance(500.0f))) { + && (start_waypoint_to_vehicle.norm() > _directional_guidance.switchDistance(500.0f))) { // we are in front of the start waypoint, fly directly to it until we are within switch distance - navigateWaypoint(start_waypoint, vehicle_pos, ground_vel, wind_vel); - return; + return navigateWaypoint(start_waypoint, vehicle_pos, ground_vel, wind_vel); } if (start_waypoint_to_end_waypoint.dot(end_waypoint_to_vehicle) > FLT_EPSILON) { @@ -3189,42 +2416,43 @@ void FixedwingPositionControl::navigateWaypoints(const Vector2f &start_waypoint, // end waypoint. however this included here as a safety precaution if any navigator (module) switch condition // is missed for any reason. in the future this logic should all be handled in one place in a dedicated // flight mode state machine. - navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); - return; + return navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); } // follow the line segment between the start and end waypoints - navigateLine(start_waypoint, end_waypoint, vehicle_pos, ground_vel, wind_vel); + return navigateLine(start_waypoint, end_waypoint, vehicle_pos, ground_vel, wind_vel); } -void FixedwingPositionControl::navigateWaypoint(const Vector2f &waypoint_pos, const Vector2f &vehicle_pos, +DirectionalGuidanceOutput FixedWingModeManager::navigateWaypoint(const Vector2f &waypoint_pos, + const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) { const Vector2f vehicle_to_waypoint = waypoint_pos - vehicle_pos; if (vehicle_to_waypoint.norm() < FLT_EPSILON) { // degenerate case: the vehicle is on top of the single waypoint. (can happen). maintain the last npfg command. - return; + return DirectionalGuidanceOutput{}; } const Vector2f unit_path_tangent = vehicle_to_waypoint.normalized(); _closest_point_on_path = waypoint_pos; const float path_curvature = 0.f; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature); + DirectionalGuidanceOutput sp = _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, + _closest_point_on_path, path_curvature); - // for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined. - _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); + return sp; } -void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2, +DirectionalGuidanceOutput FixedWingModeManager::navigateLine(const Vector2f &point_on_line_1, + const Vector2f &point_on_line_2, const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) { const Vector2f line_segment = point_on_line_2 - point_on_line_1; if (line_segment.norm() <= FLT_EPSILON) { // degenerate case: line segment has zero length. maintain the last npfg command. - return; + return DirectionalGuidanceOutput{}; } const Vector2f unit_path_tangent = line_segment.normalized(); @@ -3233,13 +2461,15 @@ void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line_1, con _closest_point_on_path = point_on_line_1 + point_1_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; const float path_curvature = 0.f; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature); + const DirectionalGuidanceOutput sp = _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, + unit_path_tangent, + _closest_point_on_path, path_curvature); - // for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined. - _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); + return sp; } -void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line, const float line_bearing, +DirectionalGuidanceOutput FixedWingModeManager::navigateLine(const Vector2f &point_on_line, + const float line_bearing, const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) { const Vector2f unit_path_tangent{cosf(line_bearing), sinf(line_bearing)}; @@ -3248,13 +2478,15 @@ void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line, const _closest_point_on_path = point_on_line + point_on_line_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; const float path_curvature = 0.f; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature); + const DirectionalGuidanceOutput sp = _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, + unit_path_tangent, + _closest_point_on_path, path_curvature); - // for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined. - _target_bearing = line_bearing; + return sp; } -void FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle_pos, +DirectionalGuidanceOutput FixedWingModeManager::navigateLoiter(const Vector2f &loiter_center, + const Vector2f &vehicle_pos, float radius, bool loiter_direction_counter_clockwise, const Vector2f &ground_vel, const Vector2f &wind_vel) { const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f; @@ -3286,50 +2518,57 @@ void FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, con // 90 deg clockwise rotation * loiter direction const Vector2f unit_path_tangent = loiter_direction_multiplier * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)}; - float path_curvature = loiter_direction_multiplier / radius; - _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); + const float path_curvature = loiter_direction_multiplier / radius; _closest_point_on_path = unit_vec_center_to_closest_pt * radius + loiter_center; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, - loiter_center + unit_vec_center_to_closest_pt * radius, path_curvature); + return _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, + loiter_center + unit_vec_center_to_closest_pt * radius, path_curvature); } -void FixedwingPositionControl::navigatePathTangent(const matrix::Vector2f &vehicle_pos, +DirectionalGuidanceOutput FixedWingModeManager::navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint, const matrix::Vector2f &tangent_setpoint, const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature) { if (tangent_setpoint.norm() <= FLT_EPSILON) { // degenerate case: no direction. maintain the last npfg command. - return; + return DirectionalGuidanceOutput{}; } const Vector2f unit_path_tangent{tangent_setpoint.normalized()}; - _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); _closest_point_on_path = position_setpoint; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, tangent_setpoint.normalized(), position_setpoint, curvature); + return _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, tangent_setpoint.normalized(), + position_setpoint, + curvature); } -void FixedwingPositionControl::navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, +DirectionalGuidanceOutput FixedWingModeManager::navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, const Vector2f &ground_vel, const Vector2f &wind_vel) { - const Vector2f unit_path_tangent = Vector2f{cosf(bearing), sinf(bearing)}; - _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); _closest_point_on_path = vehicle_pos; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, vehicle_pos, 0.0f); + return _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, vehicle_pos, 0.0f); } -int FixedwingPositionControl::task_spawn(int argc, char *argv[]) +void FixedWingModeManager::publish_lateral_guidance_status(const hrt_abstime now) { - bool vtol = false; + fixed_wing_lateral_guidance_status_s fixed_wing_lateral_guidance_status{}; - if (argc > 1) { - if (strcmp(argv[1], "vtol") == 0) { - vtol = true; - } - } + fixed_wing_lateral_guidance_status.timestamp = now; + fixed_wing_lateral_guidance_status.course_setpoint = _directional_guidance.getCourseSetpoint(); + fixed_wing_lateral_guidance_status.lateral_acceleration_ff = _directional_guidance.getLateralAccelerationSetpoint(); + fixed_wing_lateral_guidance_status.bearing_feas = _directional_guidance.getBearingFeasibility(); + fixed_wing_lateral_guidance_status.bearing_feas_on_track = _directional_guidance.getBearingFeasibilityOnTrack(); + fixed_wing_lateral_guidance_status.signed_track_error = _directional_guidance.getSignedTrackError(); + fixed_wing_lateral_guidance_status.track_error_bound = _directional_guidance.getTrackErrorBound(); + fixed_wing_lateral_guidance_status.adapted_period = _directional_guidance.getAdaptedPeriod(); + fixed_wing_lateral_guidance_status.wind_est_valid = _wind_valid; + + _fixed_wing_lateral_guidance_status_pub.publish(fixed_wing_lateral_guidance_status); +} - FixedwingPositionControl *instance = new FixedwingPositionControl(vtol); +int FixedWingModeManager::task_spawn(int argc, char *argv[]) +{ + FixedWingModeManager *instance = new FixedWingModeManager(); if (instance) { _object.store(instance); @@ -3350,12 +2589,12 @@ int FixedwingPositionControl::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -int FixedwingPositionControl::custom_command(int argc, char *argv[]) +int FixedWingModeManager::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } -int FixedwingPositionControl::print_usage(const char *reason) +int FixedWingModeManager::print_usage(const char *reason) { if (reason) { PX4_WARN("%s\n", reason); @@ -3364,33 +2603,20 @@ int FixedwingPositionControl::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -fw_pos_control is the fixed-wing position controller. +This implements the setpoint generation for all PX4-internal fixed-wing modes, height-rate control and higher. +It takes the current mode state of the vehicle as input and outputs setpoints consumed by the fixed-wing +lateral-longitudinal controller and and controllers below that (attitude, rate). )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("fw_pos_control", "controller"); + PRINT_MODULE_USAGE_NAME("fw_mode_manager", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } -float FixedwingPositionControl::getLoadFactor() -{ - float load_factor_from_bank_angle = 1.0f; - - float roll_body = Eulerf(Quatf(_att_sp.q_d)).phi(); - - if (PX4_ISFINITE(roll_body)) { - load_factor_from_bank_angle = 1.0f / math::max(cosf(roll_body), FLT_EPSILON); - } - - return load_factor_from_bank_angle; - -} - -extern "C" __EXPORT int fw_pos_control_main(int argc, char *argv[]) +extern "C" __EXPORT int fw_mode_manager_main(int argc, char *argv[]) { - return FixedwingPositionControl::main(argc, argv); + return FixedWingModeManager::main(argc, argv); } diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_mode_manager/FixedWingModeManager.hpp similarity index 72% rename from src/modules/fw_pos_control/FixedwingPositionControl.hpp rename to src/modules/fw_mode_manager/FixedWingModeManager.hpp index e56263c5e31e..81eed7d81f4f 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2025 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 @@ -33,31 +33,22 @@ /** - * @file FixedwingPositionControl.hpp - * Implementation of various fixed-wing position level navigation/control modes. - * - * The implementation for the controllers is in a separate library. This class only - * interfaces to the library. - * - * @author Lorenz Meier - * @author Thomas Gubler - * @author Andreas Antener + * @file FixedWingModeManager.hpp + * Implementation of various fixed-wing control modes. */ -#ifndef FIXEDWINGPOSITIONCONTROL_HPP_ -#define FIXEDWINGPOSITIONCONTROL_HPP_ +#ifndef FIXEDWINGMODEMANAGER_HPP_ +#define FIXEDWINGMODEMANAGER_HPP_ #include "launchdetection/LaunchDetector.h" #include "runway_takeoff/RunwayTakeoff.h" -#include +#include "ControllerConfigurationHandler.hpp" #include - #include #include #include -#include -#include +#include #include #include #include @@ -67,24 +58,26 @@ #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 @@ -97,12 +90,10 @@ #include #include #include -#include #ifdef CONFIG_FIGURE_OF_EIGHT #include "figure_eight/FigureEight.hpp" #include - #endif // CONFIG_FIGURE_OF_EIGHT using namespace launchdetection; @@ -115,12 +106,6 @@ using matrix::Vector2f; // [m] initial distance of waypoint in front of plane in heading hold mode static constexpr float HDG_HOLD_DIST_NEXT = 3000.0f; -// [m] distance (plane to waypoint in front) at which waypoints are reset in heading hold mode -static constexpr float HDG_HOLD_REACHED_DIST = 1000.0f; - -// [m] distance by which previous waypoint is set behind the plane -static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; - // [rad/s] max yawrate at which plane locks yaw for heading hold mode static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f; @@ -138,9 +123,6 @@ static constexpr hrt_abstime TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT = 10_s; // [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes static constexpr float THROTTLE_THRESH = -.9f; -// [m/s/s] slew rate limit for airspeed setpoint changes -static constexpr float ASPD_SP_SLEW_RATE = 1.f; - // [us] time after which the wind estimate is disabled if no longer updating static constexpr hrt_abstime WIND_EST_TIMEOUT = 10_s; @@ -166,24 +148,15 @@ static constexpr float MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE = 0.15f; // [s] time interval after touchdown for ramping in runway clamping constraints (touchdown is assumed at FW_LND_TD_TIME after start of flare) static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f; -// [m/s] maximum reference altitude rate threshhold -static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f; - -// [s] Timeout that has to pass in roll-constraining failsafe before warning is triggered -static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s; +// [] Stick deadzon +static constexpr float kStickDeadBand = 0.06f; -// [-] Can-run threshold needed to trigger the roll-constraining failsafe warning -static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f; - -// [s] slew rate with which we change altitude time constant -static constexpr float TECS_ALT_TIME_CONST_SLEW_RATE = 1.0f; - -class FixedwingPositionControl final : public ModuleBase, public ModuleParams, +class FixedWingModeManager final : public ModuleBase, public ModuleParams, public px4::WorkItem { public: - FixedwingPositionControl(bool vtol = false); - ~FixedwingPositionControl() override; + FixedWingModeManager(); + ~FixedWingModeManager() override; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -210,33 +183,32 @@ class FixedwingPositionControl final : public ModuleBase _attitude_sp_pub; uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; - uORB::Publication _npfg_status_pub{ORB_ID(npfg_status)}; - uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; uORB::Publication _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; - uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; uORB::Publication _launch_detection_status_pub{ORB_ID(launch_detection_status)}; uORB::PublicationMulti _orbit_status_pub{ORB_ID(orbit_status)}; uORB::Publication _landing_gear_pub {ORB_ID(landing_gear)}; uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; uORB::Publication _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)}; - uORB::PublicationData _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)}; + uORB::PublicationData _lateral_ctrl_sp_pub{ORB_ID(fixed_wing_lateral_setpoint)}; + uORB::PublicationData _longitudinal_ctrl_sp_pub{ORB_ID(fixed_wing_longitudinal_setpoint)}; + uORB::Publication _fixed_wing_lateral_guidance_status_pub{ORB_ID(fixed_wing_lateral_guidance_status)}; + uORB::Publication _fixed_wing_runway_control_pub{ORB_ID(fixed_wing_runway_control)}; manual_control_setpoint_s _manual_control_setpoint{}; position_setpoint_triplet_s _pos_sp_triplet{}; - vehicle_attitude_setpoint_s _att_sp{}; vehicle_control_mode_s _control_mode{}; vehicle_local_position_s _local_pos{}; vehicle_status_s _vehicle_status{}; + CombinedControllerConfigurationHandler _ctrl_configuration_handler; + Vector2f _lpos_where_backtrans_started; bool _position_setpoint_previous_valid{false}; @@ -272,12 +244,12 @@ class FixedwingPositionControl final : public ModuleBase _tecs_alt_time_const_slew_rate; - // VTOL / TRANSITION - bool _is_vtol_tailsitter{false}; matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN}; float _backtrans_heading{NAN}; // used to lock the initial heading for backtransition with no position control // ESTIMATOR RESET COUNTERS uint8_t _xy_reset_counter{0}; - uint8_t _z_reset_counter{0}; uint64_t _time_last_xy_reset{0}; // LATERAL-DIRECTIONAL GUIDANCE @@ -423,12 +380,7 @@ class FixedwingPositionControl final : public ModuleBase _airspeed_slew_rate_controller; - SlewRate _roll_slew_rate; - - /** - * @brief A wrapper function to call the TECS implementation - * - * @param control_interval Time since the last position control update [s] - * @param alt_sp Altitude setpoint, AMSL [m] - * @param airspeed_sp Calibrated airspeed setpoint [m/s] - * @param pitch_min_rad Nominal pitch angle command minimum [rad] - * @param pitch_max_rad Nominal pitch angle command maximum [rad] - * @param throttle_min Minimum throttle command [0,1] - * @param throttle_max Maximum throttle command [0,1] - * @param desired_max_sink_rate The desired max sink rate commandable when altitude errors are large [m/s] - * @param desired_max_climb_rate The desired max climb rate commandable when altitude errors are large [m/s] - * @param is_low_height Define whether we are in low-height flight for tighter altitude tracking - * @param disable_underspeed_detection True if underspeed detection should be disabled - * @param hgt_rate_sp Height rate setpoint [m/s] - */ - void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, float pitch_min_rad, - float pitch_max_rad, float throttle_min, float throttle_max, - const float desired_max_sink_rate, const float desired_max_climb_rate, const bool is_low_height, - bool disable_underspeed_detection = false, float hgt_rate_sp = NAN); - - /** - * @brief Constrains the roll angle setpoint near ground to avoid wingtip strike. - * - * @param roll_setpoint Unconstrained roll angle setpoint [rad] - * @param altitude Vehicle altitude (AMSL) [m] - * @param terrain_altitude Terrain altitude (AMSL) [m] - * @return Constrained roll angle setpoint [rad] - */ - float constrainRollNearGround(const float roll_setpoint, const float altitude, const float terrain_altitude) const; + float getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const; /** * @brief Calculates the touchdown position for landing with optional manual lateral adjustments. @@ -838,21 +700,6 @@ class FixedwingPositionControl final : public ModuleBase) _param_fw_gnd_spd_min, + void control_idle(); + void publish_lateral_guidance_status(const hrt_abstime now); + + float rollAngleToLateralAccel(float roll_body) const; - (ParamFloat) _param_fw_pn_r_slew_max, + DEFINE_PARAMETERS( (ParamFloat) _param_fw_r_lim, (ParamFloat) _param_npfg_period, (ParamFloat) _param_npfg_damping, (ParamBool) _param_npfg_en_period_lb, (ParamBool) _param_npfg_en_period_ub, - (ParamBool) _param_npfg_en_track_keeping, - (ParamBool) _param_npfg_en_min_gsp, - (ParamBool) _param_npfg_en_wind_reg, - (ParamFloat) _param_npfg_track_keeping_gsp_max, (ParamFloat) _param_npfg_roll_time_const, (ParamFloat) _param_npfg_switch_distance_multiplier, (ParamFloat) _param_npfg_period_safety_factor, @@ -980,80 +830,46 @@ class FixedwingPositionControl final : public ModuleBase) _param_fw_lnd_fl_pmax, (ParamFloat) _param_fw_lnd_fl_pmin, (ParamFloat) _param_fw_lnd_flalt, - (ParamFloat) _param_fw_thrtc_sc, - (ParamFloat) _param_fw_t_thr_low_hgt, (ParamBool) _param_fw_lnd_earlycfg, (ParamInt) _param_fw_lnd_useter, (ParamFloat) _param_fw_p_lim_max, (ParamFloat) _param_fw_p_lim_min, - - (ParamFloat) _param_fw_t_hrate_ff, - (ParamFloat) _param_fw_t_h_error_tc, - (ParamFloat) _param_fw_t_fast_alt_err, - (ParamFloat) _param_fw_t_thr_integ, - (ParamFloat) _param_fw_t_I_gain_pit, - (ParamFloat) _param_fw_t_ptch_damp, - (ParamFloat) _param_fw_t_rll2thr, - (ParamFloat) _param_fw_t_sink_max, - (ParamFloat) _param_fw_t_spdweight, - (ParamFloat) _param_fw_t_tas_error_tc, - (ParamFloat) _param_fw_t_thr_damping, - (ParamFloat) _param_fw_t_vert_acc, - (ParamFloat) _param_ste_rate_time_const, - (ParamFloat) _param_seb_rate_ff, (ParamFloat) _param_climbrate_target, (ParamFloat) _param_sinkrate_target, - (ParamFloat) _param_speed_standard_dev, - (ParamFloat) _param_speed_rate_standard_dev, - (ParamFloat) _param_process_noise_standard_dev, - (ParamFloat) _param_fw_thr_idle, (ParamFloat) _param_fw_thr_max, (ParamFloat) _param_fw_thr_min, - (ParamFloat) _param_fw_thr_slew_max, - (ParamFloat) _param_fw_flaps_lnd_scl, (ParamFloat) _param_fw_flaps_to_scl, (ParamFloat) _param_fw_spoilers_lnd, - (ParamInt) _param_fw_pos_stk_conf, - (ParamInt) _param_nav_gpsf_lt, (ParamFloat) _param_nav_gpsf_r, + (ParamFloat) _param_t_spdweight, // external parameters (ParamBool) _param_fw_use_airspd, - - (ParamFloat) _param_fw_psp_off, - (ParamFloat) _param_nav_loiter_rad, - (ParamFloat) _takeoff_pitch_min, - (ParamFloat) _param_nav_fw_alt_rad, - (ParamFloat) _param_fw_wing_span, (ParamFloat) _param_fw_wing_height, - - (ParamFloat) _param_rwto_npfg_period, (ParamBool) _param_rwto_nudge, - (ParamFloat) _param_fw_lnd_fl_time, (ParamFloat) _param_fw_lnd_fl_sink, (ParamFloat) _param_fw_lnd_td_time, (ParamFloat) _param_fw_lnd_td_off, (ParamInt) _param_fw_lnd_nudge, (ParamInt) _param_fw_lnd_abort, - - (ParamFloat) _param_fw_wind_arsp_sc, - (ParamFloat) _param_fw_tko_airspd, - (ParamFloat) _param_rwto_psp, - (ParamBool) _param_fw_laun_detcn_on + (ParamBool) _param_fw_laun_detcn_on, + (ParamFloat) _param_fw_airspd_max, + (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_airspd_trim, + (ParamFloat) _param_fw_t_clmb_max ) - }; -#endif // FIXEDWINGPOSITIONCONTROL_HPP_ +#endif // FIXEDWINGMODEMANAGER_HPP_ diff --git a/src/modules/fw_mode_manager/Kconfig b/src/modules/fw_mode_manager/Kconfig new file mode 100644 index 000000000000..b0c26894131f --- /dev/null +++ b/src/modules/fw_mode_manager/Kconfig @@ -0,0 +1,20 @@ +menuconfig MODULES_FW_MODE_MANAGER + bool "fw_mode_manager" + default n + ---help--- + Enable support for fw_mode_manager + +menuconfig USER_FW_MODE_MANAGER + bool "fw_mode_manager running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_FW_MODE_MANAGER + ---help--- + Put fw_mode_manager in userspace memory + +menuconfig FIGURE_OF_EIGHT + bool "fw_mode_manager figure of eight loiter support" + default n + depends on MODULES_FW_MODE_MANAGER + ---help--- + Enable support for the figure of eight loitering pattern in fixed wing. + NOTE: Enable Mavlink development support to get feedback message. diff --git a/src/modules/fw_pos_control/figure_eight/CMakeLists.txt b/src/modules/fw_mode_manager/figure_eight/CMakeLists.txt similarity index 100% rename from src/modules/fw_pos_control/figure_eight/CMakeLists.txt rename to src/modules/fw_mode_manager/figure_eight/CMakeLists.txt diff --git a/src/modules/fw_pos_control/figure_eight/FigureEight.cpp b/src/modules/fw_mode_manager/figure_eight/FigureEight.cpp similarity index 83% rename from src/modules/fw_pos_control/figure_eight/FigureEight.cpp rename to src/modules/fw_mode_manager/figure_eight/FigureEight.cpp index d0ce50a033ef..9ab672d964d3 100644 --- a/src/modules/fw_pos_control/figure_eight/FigureEight.cpp +++ b/src/modules/fw_mode_manager/figure_eight/FigureEight.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2025 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 @@ -48,11 +48,10 @@ static constexpr bool SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE{true}; static constexpr float DEFAULT_MAJOR_TO_MINOR_AXIS_RATIO{2.5f}; static constexpr float MINIMAL_FEASIBLE_MAJOR_TO_MINOR_AXIS_RATIO{2.0f}; -FigureEight::FigureEight(NPFG &npfg, matrix::Vector2f &wind_vel, float &eas2tas) : +FigureEight::FigureEight(DirectionalGuidance &npfg, matrix::Vector2f &wind_vel) : ModuleParams(nullptr), - _npfg(npfg), - _wind_vel(wind_vel), - _eas2tas(eas2tas) + _directional_guidance(npfg), + _wind_vel(wind_vel) { } @@ -64,8 +63,9 @@ void FigureEight::resetPattern() _pos_passed_circle_center_along_major_axis = false; } -void FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, - const FigureEightPatternParameters ¶meters, float target_airspeed) +DirectionalGuidanceOutput FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, + const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters) { // Sanitize inputs FigureEightPatternParameters valid_parameters{sanitizeParameters(parameters)}; @@ -81,7 +81,7 @@ void FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, const m updateSegment(curr_pos_local, valid_parameters, pattern_points); // Apply control logic based on segment - applyControl(curr_pos_local, ground_speed, valid_parameters, target_airspeed, pattern_points); + return applyControl(curr_pos_local, ground_speed, valid_parameters, pattern_points); } FigureEight::FigureEightPatternParameters FigureEight::sanitizeParameters(const FigureEightPatternParameters @@ -118,7 +118,8 @@ void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, cons const bool north_is_closer = north_center_to_pos_local.norm() < south_center_to_pos_local.norm(); // Get the normalized switch distance. - float switch_distance_normalized = _npfg.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius; + float switch_distance_normalized = _directional_guidance.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / + parameters.loiter_radius; //Far away from current figure of eight. Fly towards closer circle @@ -194,7 +195,8 @@ void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const Fi calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters); // Get the normalized switch distance. - float switch_distance_normalized = _npfg.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius; + float switch_distance_normalized = _directional_guidance.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / + parameters.loiter_radius; // Update segment if segment exit condition has been reached switch (_current_segment) { @@ -275,56 +277,60 @@ void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const Fi } } -void FigureEight::applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, - const FigureEightPatternParameters ¶meters, float target_airspeed, - const FigureEightPatternPoints &pattern_points) +DirectionalGuidanceOutput FigureEight::applyControl(const matrix::Vector2f &curr_pos_local, + const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters, + const FigureEightPatternPoints &pattern_points) { Vector2f center_to_pos_local; calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters); switch (_current_segment) { case FigureEightSegment::SEGMENT_CIRCLE_NORTH: { - applyCircle(NORTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_north_circle_offset, curr_pos_local, - ground_speed, parameters, target_airspeed); + return applyCircle(NORTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_north_circle_offset, curr_pos_local, + ground_speed, parameters); } break; case FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST: { // Follow path from north-east to south-west - applyLine(pattern_points.normalized_north_exit_offset, pattern_points.normalized_south_entry_offset, curr_pos_local, - ground_speed, parameters, target_airspeed); + return applyLine(pattern_points.normalized_north_exit_offset, pattern_points.normalized_south_entry_offset, + curr_pos_local, + ground_speed, parameters); } break; case FigureEightSegment::SEGMENT_CIRCLE_SOUTH: { - applyCircle(SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_south_circle_offset, curr_pos_local, - ground_speed, parameters, target_airspeed); + return applyCircle(SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_south_circle_offset, curr_pos_local, + ground_speed, parameters); } break; case FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST: { // follow path from south-east to north-west - applyLine(pattern_points.normalized_south_exit_offset, pattern_points.normalized_north_entry_offset, curr_pos_local, - ground_speed, parameters, target_airspeed); + return applyLine(pattern_points.normalized_south_exit_offset, pattern_points.normalized_north_entry_offset, + curr_pos_local, + ground_speed, parameters); } break; case FigureEightSegment::SEGMENT_POINT_SOUTHWEST: { // Follow path from current position to south-west - applyLine(center_to_pos_local, pattern_points.normalized_south_entry_offset, curr_pos_local, - ground_speed, parameters, target_airspeed); + return applyLine(center_to_pos_local, pattern_points.normalized_south_entry_offset, curr_pos_local, + ground_speed, parameters); } break; case FigureEightSegment::SEGMENT_POINT_NORTHWEST: { // Follow path from current position to north-west - applyLine(center_to_pos_local, pattern_points.normalized_north_entry_offset, curr_pos_local, - ground_speed, parameters, target_airspeed); + return applyLine(center_to_pos_local, pattern_points.normalized_north_entry_offset, curr_pos_local, + ground_speed, parameters); } break; case FigureEightSegment::SEGMENT_UNDEFINED: default: + return DirectionalGuidanceOutput{}; break; } } @@ -356,9 +362,10 @@ float FigureEight::calculateRotationAngle(const FigureEightPatternParameters &pa return yaw_rotation; } -void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset, - const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, - const FigureEightPatternParameters ¶meters, float target_airspeed) +DirectionalGuidanceOutput FigureEight::applyCircle(bool loiter_direction_counter_clockwise, + const matrix::Vector2f &normalized_circle_offset, + const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters) { const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f; @@ -366,8 +373,6 @@ void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const mat Vector2f circle_offset_rotated = Dcm2f(calculateRotationAngle(parameters)) * circle_offset; Vector2f circle_center = parameters.center_pos_local + circle_offset_rotated; - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); const Vector2f vector_center_to_vehicle = curr_pos_local - circle_center; const float dist_to_center = vector_center_to_vehicle.norm(); @@ -392,16 +397,14 @@ void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const mat float path_curvature = loiter_direction_multiplier / parameters.loiter_minor_radius; _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); _closest_point_on_path = unit_vec_center_to_closest_pt * parameters.loiter_minor_radius + circle_center; - _npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, unit_path_tangent, - _closest_point_on_path, path_curvature); + return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, unit_path_tangent, + _closest_point_on_path, path_curvature); - _roll_setpoint = _npfg.getRollSetpoint(); - _indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas; } -void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset, - const matrix::Vector2f &normalized_line_end_offset, const matrix::Vector2f &curr_pos_local, - const matrix::Vector2f &ground_speed, const FigureEightPatternParameters ¶meters, float target_airspeed) +DirectionalGuidanceOutput FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset, + const matrix::Vector2f &normalized_line_end_offset, const matrix::Vector2f &curr_pos_local, + const matrix::Vector2f &ground_speed, const FigureEightPatternParameters ¶meters) { const Dcm2f rotation_matrix(calculateRotationAngle(parameters)); @@ -414,15 +417,12 @@ void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset const Vector2f end_offset_rotated = rotation_matrix * end_offset; const Vector2f line_segment_end_position = parameters.center_pos_local + end_offset_rotated; - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); const Vector2f path_tangent = line_segment_end_position - line_segment_start_position; const Vector2f unit_path_tangent = path_tangent.normalized(); _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); const Vector2f vector_A_to_vehicle = curr_pos_local - line_segment_start_position; _closest_point_on_path = line_segment_start_position + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; - _npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(), line_segment_start_position, - 0.0f); - _roll_setpoint = _npfg.getRollSetpoint(); - _indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas; + return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(), + line_segment_start_position, + 0.0f); } diff --git a/src/modules/fw_pos_control/figure_eight/FigureEight.hpp b/src/modules/fw_mode_manager/figure_eight/FigureEight.hpp similarity index 81% rename from src/modules/fw_pos_control/figure_eight/FigureEight.hpp rename to src/modules/fw_mode_manager/figure_eight/FigureEight.hpp index b3c55c840b47..297e07942c85 100644 --- a/src/modules/fw_pos_control/figure_eight/FigureEight.hpp +++ b/src/modules/fw_mode_manager/figure_eight/FigureEight.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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 @@ -45,7 +45,7 @@ #include #include -#include "lib/npfg/npfg.hpp" +#include "lib/npfg/DirectionalGuidance.hpp" class FigureEight : public ModuleParams { @@ -87,9 +87,8 @@ class FigureEight : public ModuleParams * * @param[in] npfg is the reference to the parent npfg object. * @param[in] wind_vel is the reference to the parent wind velocity [m/s]. - * @param[in] eas2tas is the reference to the parent indicated airspeed to true airspeed conversion. */ - FigureEight(NPFG &npfg, matrix::Vector2f &wind_vel, float &eas2tas); + FigureEight(DirectionalGuidance &directional_guidance, matrix::Vector2f &wind_vel); /** * @brief reset the figure eight pattern. @@ -100,27 +99,14 @@ class FigureEight : public ModuleParams void resetPattern(); /** - * @brief Update roll and airspeed setpoint. + * @brief Update roll setpoint * * @param[in] curr_pos_local is the current local position of the vehicle in [m]. * @param[in] ground_speed is the current ground speed of the vehicle in [m/s]. * @param[in] parameters is the parameter set defining the figure eight shape. - * @param[in] target_airspeed is the current targeted indicated airspeed [m/s]. */ - void updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, - const FigureEightPatternParameters ¶meters, float target_airspeed); - /** - * @brief Get the roll setpoint - * - * @return the roll setpoint in [rad]. - */ - float getRollSetpoint() const {return _roll_setpoint;}; - /** - * @brief Get the indicated airspeed setpoint - * - * @return the indicated airspeed setpoint in [m/s]. - */ - float getAirspeedSetpoint() const {return _indicated_airspeed_setpoint;}; + DirectionalGuidanceOutput updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters); /** * @brief Get the target bearing of current point on figure of eight * @@ -134,7 +120,6 @@ class FigureEight : public ModuleParams */ matrix::Vector2f getClosestPoint() const {return _closest_point_on_path;}; - private: /** * @brief @@ -172,12 +157,11 @@ class FigureEight : public ModuleParams * @param[in] curr_pos_local is the current local position of the vehicle in [m]. * @param[in] ground_speed is the current ground speed of the vehicle in [m/s]. * @param[in] parameters is the parameter set defining the figure eight shape. - * @param[in] target_airspeed is the current targeted indicated airspeed [m/s]. * @param[in] pattern_points are the relevant points defining the figure eight pattern. */ - void applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, - const FigureEightPatternParameters ¶meters, float target_airspeed, - const FigureEightPatternPoints &pattern_points); + DirectionalGuidanceOutput applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters, + const FigureEightPatternPoints &pattern_points); /** * @brief Update active segment. * @@ -212,11 +196,11 @@ class FigureEight : public ModuleParams * @param[in] curr_pos_local is the current local position of the vehicle in [m]. * @param[in] ground_speed is the current ground speed of the vehicle in [m/s]. * @param[in] parameters is the parameter set defining the figure eight shape. - * @param[in] target_airspeed is the current targeted indicated airspeed [m/s]. */ - void applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset, - const matrix::Vector2f &curr_pos_local, - const matrix::Vector2f &ground_speed, const FigureEightPatternParameters ¶meters, float target_airspeed); + DirectionalGuidanceOutput applyCircle(bool loiter_direction_counter_clockwise, + const matrix::Vector2f &normalized_circle_offset, + const matrix::Vector2f &curr_pos_local, + const matrix::Vector2f &ground_speed, const FigureEightPatternParameters ¶meters); /** * @brief Apply path lateral control * @@ -225,18 +209,18 @@ class FigureEight : public ModuleParams * @param[in] curr_pos_local is the current local position of the vehicle in [m]. * @param[in] ground_speed is the current ground speed of the vehicle in [m/s]. * @param[in] parameters is the parameter set defining the figure eight shape. - * @param[in] target_airspeed is the current targeted indicated airspeed [m/s]. */ - void applyLine(const matrix::Vector2f &normalized_line_start_offset, const matrix::Vector2f &normalized_line_end_offset, - const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, - const FigureEightPatternParameters ¶meters, float target_airspeed); + DirectionalGuidanceOutput applyLine(const matrix::Vector2f &normalized_line_start_offset, + const matrix::Vector2f &normalized_line_end_offset, + const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed, + const FigureEightPatternParameters ¶meters); private: /** * @brief npfg lateral control object. * */ - NPFG &_npfg; + DirectionalGuidance &_directional_guidance; /** * @brief Wind velocity in [m/s]. @@ -244,24 +228,9 @@ class FigureEight : public ModuleParams */ const matrix::Vector2f &_wind_vel; /** - * @brief Conversion factor from indicated to true airspeed. - * - */ - const float &_eas2tas; - /** - * @brief Roll setpoint in [rad]. - * - */ - float _roll_setpoint; - /** - * @brief Indicated airspeed setpoint in [m/s]. - * - */ - float _indicated_airspeed_setpoint; - /** - * @brief active figure eight position setpoint. - * - */ + * @brief active figure eight position setpoint. + * + */ FigureEightPatternParameters _active_parameters; /** diff --git a/src/modules/fw_pos_control/fw_path_navigation_params.c b/src/modules/fw_mode_manager/fw_mode_manager_params.c similarity index 67% rename from src/modules/fw_pos_control/fw_path_navigation_params.c rename to src/modules/fw_mode_manager/fw_mode_manager_params.c index 2e2005997a7d..2269ba79f157 100644 --- a/src/modules/fw_pos_control/fw_path_navigation_params.c +++ b/src/modules/fw_mode_manager/fw_mode_manager_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2025 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 @@ -31,20 +31,6 @@ * ****************************************************************************/ -/** - * Path navigation roll slew rate limit. - * - * Maximum change in roll angle setpoint per second. - * Applied in all Auto modes, plus manual Position & Altitude modes. - * - * @unit deg/s - * @min 0 - * @decimal 0 - * @increment 1 - * @group FW Path Control - */ -PARAM_DEFINE_FLOAT(FW_PN_R_SLEW_MAX, 90.0f); - /** * NPFG period * @@ -93,48 +79,6 @@ PARAM_DEFINE_INT32(NPFG_LB_PERIOD, 1); */ PARAM_DEFINE_INT32(NPFG_UB_PERIOD, 1); -/** - * Enable track keeping excess wind handling logic. - * - * @boolean - * @group FW NPFG Control - */ -PARAM_DEFINE_INT32(NPFG_TRACK_KEEP, 1); - -/** - * Enable minimum forward ground speed maintaining excess wind handling logic - * - * @boolean - * @group FW NPFG Control - */ -PARAM_DEFINE_INT32(NPFG_EN_MIN_GSP, 1); - -/** - * Enable wind excess regulation. - * - * Disabling this parameter further disables all other airspeed incrementation options. - * - * @boolean - * @group FW NPFG Control - */ -PARAM_DEFINE_INT32(NPFG_WIND_REG, 1); - -/** - * Maximum, minimum forward ground speed for track keeping in excess wind - * - * The maximum value of the minimum forward ground speed that may be commanded - * by the track keeping excess wind handling logic. Commanded in full at the normalized - * track error fraction of the track error boundary and reduced to zero on track. - * - * @unit m/s - * @min 0.0 - * @max 10.0 - * @decimal 1 - * @increment 0.5 - * @group FW NPFG Control - */ -PARAM_DEFINE_FLOAT(NPFG_GSP_MAX_TK, 5.0f); - /** * Roll time constant * @@ -179,19 +123,6 @@ PARAM_DEFINE_FLOAT(NPFG_SW_DST_MLT, 0.32f); PARAM_DEFINE_FLOAT(NPFG_PERIOD_SF, 1.5f); -/** - * Throttle max slew rate - * - * Maximum slew rate for the commanded throttle - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.01 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f); - /** * Minimum pitch angle setpoint * @@ -202,7 +133,7 @@ PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f); * @max 0.0 * @decimal 1 * @increment 0.5 - * @group FW TECS + * @group FW General */ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -30.0f); @@ -216,7 +147,7 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -30.0f); * @max 60.0 * @decimal 1 * @increment 0.5 - * @group FW TECS + * @group FW General */ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 30.0f); @@ -230,7 +161,7 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 30.0f); * @max 65.0 * @decimal 1 * @increment 0.5 - * @group FW Path Control + * @group FW General */ PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f); @@ -245,7 +176,7 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f); * @max 1.0 * @decimal 2 * @increment 0.01 - * @group FW TECS + * @group FW General */ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); @@ -261,7 +192,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); * @max 1.0 * @decimal 2 * @increment 0.01 - * @group FW TECS + * @group FW General */ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); @@ -275,7 +206,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); * @max 0.4 * @decimal 2 * @increment 0.01 - * @group FW TECS + * @group FW General */ PARAM_DEFINE_FLOAT(FW_THR_IDLE, 0.0f); @@ -302,7 +233,7 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); * @max 30.0 * @decimal 1 * @increment 0.5 - * @group FW Path Control + * @group FW Auto Takeoff */ PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f); @@ -317,7 +248,7 @@ PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f); * @min -1.0 * @decimal 1 * @increment 0.1 - * @group FW TECS + * @group FW Auto Takeoff */ PARAM_DEFINE_FLOAT(FW_TKO_AIRSPD, -1.0f); @@ -423,157 +354,6 @@ PARAM_DEFINE_FLOAT(FW_LND_AIRSPD, -1.f); */ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f); -/** - * Low-height threshold for tighter altitude tracking - * - * Height above ground threshold below which tighter altitude - * tracking gets enabled (see FW_LND_THRTC_SC). Below this height, TECS smoothly - * (1 sec / sec) transitions the altitude tracking time constant from FW_T_ALT_TC - * to FW_LND_THRTC_SC*FW_T_ALT_TC. - * - * -1 to disable. - * - * @unit m - * @min -1 - * @decimal 0 - * @increment 1 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_THR_LOW_HGT, -1.f); - -/* - * TECS parameters - * - */ - -/** - * Maximum descent rate - * - * @unit m/s - * @min 1.0 - * @max 15.0 - * @decimal 1 - * @increment 0.5 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); - -/** - * Throttle damping factor - * - * This is the damping gain for the throttle demand loop. - * - * @min 0.0 - * @max 1.0 - * @decimal 3 - * @increment 0.01 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_THR_DAMPING, 0.05f); - -/** - * Integrator gain throttle - * - * Increase it to trim out speed and height offsets faster, - * with the downside of possible overshoots and oscillations. - * - * @min 0.0 - * @max 1.0 - * @decimal 3 - * @increment 0.005 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_THR_INTEG, 0.02f); - -/** - * Integrator gain pitch - * - * Increase it to trim out speed and height offsets faster, - * with the downside of possible overshoots and oscillations. - * - * @min 0.0 - * @max 2.0 - * @decimal 2 - * @increment 0.05 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f); - -/** - * Maximum vertical acceleration - * - * This is the maximum vertical acceleration - * either up or down that the controller will use to correct speed - * or height errors. - * - * @unit m/s^2 - * @min 1.0 - * @max 10.0 - * @decimal 1 - * @increment 0.5 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); - -/** - * Airspeed measurement standard deviation - * - * For the airspeed filter in TECS. - * - * @unit m/s - * @min 0.01 - * @max 10.0 - * @decimal 2 - * @increment 0.1 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_SPD_STD, 0.07f); - -/** - * Airspeed rate measurement standard deviation - * - * For the airspeed filter in TECS. - * - * @unit m/s^2 - * @min 0.01 - * @max 10.0 - * @decimal 2 - * @increment 0.1 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_SPD_DEV_STD, 0.2f); - -/** - * Process noise standard deviation for the airspeed rate - * - * This is defining the noise in the airspeed rate for the constant airspeed rate model - * of the TECS airspeed filter. - * - * @unit m/s^2 - * @min 0.01 - * @max 10.0 - * @decimal 2 - * @increment 0.1 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_SPD_PRC_STD, 0.2f); - - -/** - * Roll -> Throttle feedforward - * - * Is used to compensate for the additional drag created by turning. - * Increase this gain if the aircraft initially loses energy in turns - * and reduce if the aircraft initially gains energy in turns. - * - * @min 0.0 - * @max 20.0 - * @decimal 1 - * @increment 0.5 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f); - /** * Speed <--> Altitude weight * @@ -586,79 +366,10 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f); * @max 2.0 * @decimal 1 * @increment 1.0 - * @group FW TECS + * @group FW General */ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); -/** - * Pitch damping gain - * - * @min 0.0 - * @max 2.0 - * @decimal 2 - * @increment 0.1 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.1f); - -/** - * Altitude error time constant. - * - * @min 2.0 - * @decimal 2 - * @increment 0.5 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_ALT_TC, 5.0f); - -/** - * Fast descend: minimum altitude error - * - * Minimum altitude error needed to descend with max airspeed and minimal throttle. - * A negative value disables fast descend. - * - * @min -1.0 - * @decimal 0 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_F_ALT_ERR, -1.0f); - -/** - * Height rate feed forward - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.3f); - -/** - * True airspeed error time constant. - * - * @min 2.0 - * @decimal 2 - * @increment 0.5 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_TAS_TC, 5.0f); - -/** - * Minimum groundspeed - * - * The controller will increase the commanded airspeed to maintain - * this minimum groundspeed to the next waypoint. - * - * @unit m/s - * @min 0.0 - * @max 40 - * @decimal 1 - * @increment 0.5 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f); - /** * Custom stick configuration * @@ -668,35 +379,10 @@ PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f); * @max 3 * @bit 0 Alternative stick configuration (height rate on throttle stick, airspeed on pitch stick) * @bit 1 Enable airspeed setpoint via sticks in altitude and position flight mode - * @group FW Path Control + * @group FW General */ PARAM_DEFINE_INT32(FW_POS_STK_CONF, 2); -/** - * Specific total energy rate first order filter time constant. - * - * This filter is applied to the specific total energy rate used for throttle damping. - * - * @min 0.0 - * @max 2 - * @decimal 2 - * @increment 0.01 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_STE_R_TC, 0.4f); - -/** - * Specific total energy balance rate feedforward gain. - * - * - * @min 0.5 - * @max 3 - * @decimal 2 - * @increment 0.01 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_SEB_R_FF, 1.0f); - /** * Default target climbrate. * @@ -708,7 +394,7 @@ PARAM_DEFINE_FLOAT(FW_T_SEB_R_FF, 1.0f); * @max 15 * @decimal 2 * @increment 0.01 - * @group FW TECS + * @group FW General */ PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP, 3.0f); @@ -723,7 +409,7 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP, 3.0f); * @max 15 * @decimal 2 * @increment 0.01 - * @group FW TECS + * @group FW General */ PARAM_DEFINE_FLOAT(FW_T_SINK_R_SP, 2.0f); @@ -737,7 +423,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_R_SP, 2.0f); * @unit s * @min 0 * @max 3600 - * @group Mission + * @group FW General */ PARAM_DEFINE_INT32(FW_GPSF_LT, 30); @@ -751,7 +437,7 @@ PARAM_DEFINE_INT32(FW_GPSF_LT, 30); * @max 30.0 * @decimal 1 * @increment 0.5 - * @group Mission + * @group FW General */ PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f); @@ -765,7 +451,7 @@ PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f); * @min 0.1 * @decimal 1 * @increment 0.1 - * @group FW Geometry + * @group FW General */ PARAM_DEFINE_FLOAT(FW_WING_SPAN, 3.0); @@ -779,7 +465,7 @@ PARAM_DEFINE_FLOAT(FW_WING_SPAN, 3.0); * @min 0.0 * @decimal 1 * @increment 1 - * @group FW Geometry + * @group FW General */ PARAM_DEFINE_FLOAT(FW_WING_HEIGHT, 0.5); @@ -886,20 +572,6 @@ PARAM_DEFINE_INT32(FW_LND_NUDGE, 2); */ PARAM_DEFINE_INT32(FW_LND_ABORT, 3); -/** - * Wind-based airspeed scaling factor - * - * Multiplying this factor with the current absolute wind estimate gives the airspeed offset - * added to the minimum airspeed setpoint limit. This helps to make the - * system more robust against disturbances (turbulence) in high wind. - * - * @min 0 - * @decimal 2 - * @increment 0.01 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.f); - /** * Fixed-wing launch detection * @@ -907,7 +579,7 @@ PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.f); * Not compatible with runway takeoff. * * @boolean - * @group FW Launch detection + * @group FW Auto Takeoff */ PARAM_DEFINE_INT32(FW_LAUN_DETCN_ON, 0); @@ -922,7 +594,7 @@ PARAM_DEFINE_INT32(FW_LAUN_DETCN_ON, 0); * @max 1.0 * @decimal 2 * @increment 0.01 - * @group FW Rate Control + * @group FW Auto Takeoff */ PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f); @@ -937,7 +609,7 @@ PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f); * @max 1.0 * @decimal 2 * @increment 0.01 - * @group FW Rate Control + * @group FW Auto Landing */ PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f); @@ -949,6 +621,6 @@ PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f); * @max 1.0 * @decimal 2 * @increment 0.01 - * @group FW Attitude Control + * @group FW Auto Landing */ PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f); diff --git a/src/modules/fw_pos_control/launchdetection/CMakeLists.txt b/src/modules/fw_mode_manager/launchdetection/CMakeLists.txt similarity index 100% rename from src/modules/fw_pos_control/launchdetection/CMakeLists.txt rename to src/modules/fw_mode_manager/launchdetection/CMakeLists.txt diff --git a/src/modules/fw_pos_control/launchdetection/LaunchDetector.cpp b/src/modules/fw_mode_manager/launchdetection/LaunchDetector.cpp similarity index 100% rename from src/modules/fw_pos_control/launchdetection/LaunchDetector.cpp rename to src/modules/fw_mode_manager/launchdetection/LaunchDetector.cpp diff --git a/src/modules/fw_pos_control/launchdetection/LaunchDetector.h b/src/modules/fw_mode_manager/launchdetection/LaunchDetector.h similarity index 100% rename from src/modules/fw_pos_control/launchdetection/LaunchDetector.h rename to src/modules/fw_mode_manager/launchdetection/LaunchDetector.h diff --git a/src/modules/fw_pos_control/launchdetection/launchdetection_params.c b/src/modules/fw_mode_manager/launchdetection/launchdetection_params.c similarity index 96% rename from src/modules/fw_pos_control/launchdetection/launchdetection_params.c rename to src/modules/fw_mode_manager/launchdetection/launchdetection_params.c index 63e14bedbdbc..eee3d7ccef77 100644 --- a/src/modules/fw_pos_control/launchdetection/launchdetection_params.c +++ b/src/modules/fw_mode_manager/launchdetection/launchdetection_params.c @@ -48,7 +48,7 @@ * @min 0 * @decimal 1 * @increment 0.5 - * @group FW Launch detection + * @group FW Auto Takeoff */ PARAM_DEFINE_FLOAT(FW_LAUN_AC_THLD, 30.0f); @@ -62,7 +62,7 @@ PARAM_DEFINE_FLOAT(FW_LAUN_AC_THLD, 30.0f); * @max 5.0 * @decimal 2 * @increment 0.05 - * @group FW Launch detection + * @group FW Auto Takeoff */ PARAM_DEFINE_FLOAT(FW_LAUN_AC_T, 0.05f); @@ -76,6 +76,6 @@ PARAM_DEFINE_FLOAT(FW_LAUN_AC_T, 0.05f); * @max 10.0 * @decimal 1 * @increment 0.5 - * @group FW Launch detection + * @group FW Auto Takeoff */ PARAM_DEFINE_FLOAT(FW_LAUN_MOT_DEL, 0.0f); diff --git a/src/modules/fw_pos_control/runway_takeoff/CMakeLists.txt b/src/modules/fw_mode_manager/runway_takeoff/CMakeLists.txt similarity index 100% rename from src/modules/fw_pos_control/runway_takeoff/CMakeLists.txt rename to src/modules/fw_mode_manager/runway_takeoff/CMakeLists.txt diff --git a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.cpp similarity index 82% rename from src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp rename to src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.cpp index 7cfc23b4384c..498a5332b1f3 100644 --- a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.cpp @@ -51,10 +51,8 @@ using namespace time_literals; namespace runwaytakeoff { -void RunwayTakeoff::init(const hrt_abstime &time_now, const float initial_yaw, const matrix::Vector2d &start_pos_global) +void RunwayTakeoff::init(const hrt_abstime &time_now) { - initial_yaw_ = initial_yaw; - start_pos_global_ = start_pos_global; takeoff_state_ = RunwayTakeoffState::THROTTLE_RAMP; initialized_ = true; time_initialized_ = time_now; @@ -99,42 +97,26 @@ void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airs } } -bool RunwayTakeoff::controlYaw() -{ - // keep controlling yaw directly until we start navigation - return takeoff_state_ < RunwayTakeoffState::CLIMBOUT; -} - -float RunwayTakeoff::getPitch(float external_pitch_setpoint) +float RunwayTakeoff::getPitch() { if (takeoff_state_ <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { return math::radians(param_rwto_psp_.get()); } - return external_pitch_setpoint; + return NAN; } -float RunwayTakeoff::getRoll(float external_roll_setpoint) +float RunwayTakeoff::getRoll() { // until we have enough ground clearance, set roll to 0 if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) { return 0.0f; } - return external_roll_setpoint; -} - -float RunwayTakeoff::getYaw(float external_yaw_setpoint) -{ - if (param_rwto_hdg_.get() == 0 && takeoff_state_ < RunwayTakeoffState::CLIMBOUT) { - return initial_yaw_; - - } else { - return external_yaw_setpoint; - } + return NAN; } -float RunwayTakeoff::getThrottle(const float idle_throttle, const float external_throttle_setpoint) const +float RunwayTakeoff::getThrottle(const float idle_throttle) const { float throttle = idle_throttle; @@ -147,30 +129,18 @@ float RunwayTakeoff::getThrottle(const float idle_throttle, const float external break; case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - throttle = param_rwto_max_thr_.get(); - - break; - case RunwayTakeoffState::CLIMBOUT: - // ramp in throttle setpoint over takeoff rotation transition time - throttle = interpolateValuesOverAbsoluteTime(param_rwto_max_thr_.get(), external_throttle_setpoint, takeoff_time_, - param_rwto_rot_time_.get()); + throttle = param_rwto_max_thr_.get(); break; case RunwayTakeoffState::FLY: - throttle = external_throttle_setpoint; + throttle = NAN; } return throttle; } -bool RunwayTakeoff::resetIntegrators() -{ - // reset integrators if we're still on runway - return takeoff_state_ < RunwayTakeoffState::CLIMBOUT; -} - float RunwayTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch) const { if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) { diff --git a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.h similarity index 72% rename from src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h rename to src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.h index 35e463077ec3..22fa8a51ecc6 100644 --- a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.h @@ -70,10 +70,8 @@ class __EXPORT RunwayTakeoff : public ModuleParams * @brief Initializes the state machine. * * @param time_now Absolute time since system boot [us] - * @param initial_yaw Vehicle yaw angle at time of initialization [us] - * @param start_pos_global Vehicle global (lat, lon) position at time of initialization [deg] */ - void init(const hrt_abstime &time_now, const float initial_yaw, const matrix::Vector2d &start_pos_global); + void init(const hrt_abstime &time_now); /** * @brief Updates the state machine based on the current vehicle condition. @@ -103,37 +101,14 @@ class __EXPORT RunwayTakeoff : public ModuleParams bool runwayTakeoffEnabled() { return param_rwto_tkoff_.get(); } /** - * @return Initial vehicle yaw angle [rad] - */ - float getInitYaw() { return initial_yaw_; } - - /** - * @return The vehicle should control yaw via rudder or nose gear - */ - bool controlYaw(); - - /** - * @param external_pitch_setpoint Externally commanded pitch angle setpoint (usually from TECS) [rad] * @return Pitch angle setpoint (limited while plane is on runway) [rad] */ - float getPitch(float external_pitch_setpoint); + float getPitch(); /** - * @param external_roll_setpoint Externally commanded roll angle setpoint (usually from path navigation) [rad] * @return Roll angle setpoint [rad] */ - float getRoll(float external_roll_setpoint); - - /** - * @brief Returns the appropriate yaw angle setpoint. - * - * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the runway. - * When it has enough ground clearance we start navigation towards WP. - * - * @param external_yaw_setpoint Externally commanded yaw angle setpoint [rad] - * @return Yaw angle setpoint [rad] - */ - float getYaw(float external_yaw_setpoint); + float getRoll(); /** * @brief Returns the throttle setpoint. @@ -142,10 +117,9 @@ class __EXPORT RunwayTakeoff : public ModuleParams * ramps from RWTO_MAX_THR to the externally defined throttle setting over the takeoff rotation time * * @param idle_throttle normalized [0,1] - * @param external_throttle_setpoint Externally commanded throttle setpoint (usually from TECS), normalized [0,1] * @return Throttle setpoint, normalized [0,1] */ - float getThrottle(const float idle_throttle, const float external_throttle_setpoint) const; + float getThrottle(const float idle_throttle) const; /** * @param min_pitch_in_climbout Minimum pitch angle during climbout [rad] @@ -160,20 +134,9 @@ class __EXPORT RunwayTakeoff : public ModuleParams */ float getMaxPitch(const float max_pitch) const; - /** - * @return Runway takeoff starting position in global frame (lat, lon) [deg] - */ - const matrix::Vector2d &getStartPosition() const { return start_pos_global_; }; - // NOTE: this is only to be used for mistaken mode transitions to takeoff while already in air void forceSetFlyState() { takeoff_state_ = RunwayTakeoffState::FLY; } - /** - * @return If the attitude / rate control integrators should be continually reset. - * This is the case during ground roll. - */ - bool resetIntegrators(); - /** * @brief Reset the state machine. */ @@ -212,22 +175,8 @@ class __EXPORT RunwayTakeoff : public ModuleParams */ hrt_abstime takeoff_time_{0}; - /** - * Initial yaw of the vehicle on first pass through the runway takeoff state machine. - * used for heading hold mode. [rad] - */ - float initial_yaw_{0.f}; - - /** - * The global (lat, lon) position of the vehicle on first pass through the runway takeoff state machine. The - * takeoff path emanates from this point to correct for any GNSS uncertainty from the planned takeoff point. The - * vehicle should accordingly be set on the center of the runway before engaging the mission. [deg] - */ - matrix::Vector2d start_pos_global_{}; - DEFINE_PARAMETERS( (ParamBool) param_rwto_tkoff_, - (ParamInt) param_rwto_hdg_, (ParamFloat) param_rwto_max_thr_, (ParamFloat) param_rwto_psp_, (ParamFloat) param_rwto_ramp_time_, diff --git a/src/modules/fw_pos_control/runway_takeoff/runway_takeoff_params.c b/src/modules/fw_mode_manager/runway_takeoff/runway_takeoff_params.c similarity index 85% rename from src/modules/fw_pos_control/runway_takeoff/runway_takeoff_params.c rename to src/modules/fw_mode_manager/runway_takeoff/runway_takeoff_params.c index 766cd8f54e45..68afdeffa126 100644 --- a/src/modules/fw_pos_control/runway_takeoff/runway_takeoff_params.c +++ b/src/modules/fw_mode_manager/runway_takeoff/runway_takeoff_params.c @@ -48,22 +48,7 @@ PARAM_DEFINE_INT32(RWTO_TKOFF, 0); /** - * Specifies which heading should be held during the runway takeoff ground roll. - * - * 0: airframe heading when takeoff is initiated - * 1: position control along runway direction (bearing defined from vehicle position on takeoff initiation to MAV_CMD_TAKEOFF - * position defined by operator) - * - * @value 0 Airframe - * @value 1 Runway - * @min 0 - * @max 1 - * @group Runway Takeoff - */ -PARAM_DEFINE_INT32(RWTO_HDG, 0); - -/** - * Max throttle during runway takeoff. + * Throttle during runway takeoff. * * @unit norm * @min 0.0 @@ -102,18 +87,6 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); */ PARAM_DEFINE_FLOAT(RWTO_RAMP_TIME, 2.0f); -/** - * NPFG period while steering on runway - * - * @unit s - * @min 1.0 - * @max 100.0 - * @decimal 1 - * @increment 0.1 - * @group Runway Takeoff - */ -PARAM_DEFINE_FLOAT(RWTO_NPFG_PERIOD, 5.0f); - /** * Enable use of yaw stick for nudging the wheel during runway ground roll * diff --git a/src/modules/fw_pos_control/Kconfig b/src/modules/fw_pos_control/Kconfig deleted file mode 100644 index d0bffb8dd861..000000000000 --- a/src/modules/fw_pos_control/Kconfig +++ /dev/null @@ -1,20 +0,0 @@ -menuconfig MODULES_FW_POS_CONTROL - bool "fw_pos_control" - default n - ---help--- - Enable support for fw_pos_control - -menuconfig USER_FW_POS_CONTROL - bool "fw_pos_control running as userspace module" - default n - depends on BOARD_PROTECTED && MODULES_FW_POS_CONTROL - ---help--- - Put fw_pos_control in userspace memory - -menuconfig FIGURE_OF_EIGHT - bool "fw_pos_control figure of eight loiter support" - default n - depends on MODULES_FW_POS_CONTROL - ---help--- - Enable support for the figure of eight loitering pattern in fixed wing. - NOTE: Enable Mavlink development support to get feedback message. diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 544ecae871ef..3d011cbc5c4b 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -94,7 +94,6 @@ void LoggedTopics::add_default_topics() add_topic("mission_result"); add_topic("navigator_mission_item"); add_topic("navigator_status"); - add_topic("npfg_status", 100); add_topic("offboard_control_mode", 100); add_topic("onboard_computer_status", 10); add_topic("parameter_update"); @@ -149,6 +148,13 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_status"); add_optional_topic("vtol_vehicle_status", 200); add_topic("wind", 1000); + add_topic("fixed_wing_lateral_setpoint"); + add_topic("fixed_wing_longitudinal_setpoint"); + add_topic("longitudinal_control_configuration"); + add_topic("lateral_control_configuration"); + add_optional_topic("fixed_wing_lateral_guidance_status", 100); + add_optional_topic("fixed_wing_lateral_status", 100); + add_optional_topic("fixed_wing_runway_control", 100); // multi topics add_optional_topic_multi("actuator_outputs", 100, 3); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 9c1e72b1a5c8..253f80960f58 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -63,8 +63,6 @@ TEST(PositionControlTest, EmptySetpoint) EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f); EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f)); EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f)); - EXPECT_EQ(attitude.reset_integral, false); - EXPECT_EQ(attitude.fw_control_yaw_wheel, false); } class PositionControlBasicTest : public ::testing::Test diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 24c30d885070..a1e33be86167 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -163,5 +163,17 @@ subscriptions: - topic: /fmu/in/aux_global_position type: px4_msgs::msg::VehicleGlobalPosition + - topic: /fmu/in/fixed_wing_longitudinal_setpoint + type: px4_msgs::msg::FixedWingLongitudinalSetpoint + + - topic: /fmu/in/fixed_wing_lateral_setpoint + type: px4_msgs::msg::FixedWingLateralSetpoint + + - topic: /fmu/in/longitudinal_control_configuration + type: px4_msgs::msg::LongitudinalControlConfiguration + + - topic: /fmu/in/lateral_control_configuration + type: px4_msgs::msg::LateralControlConfiguration + # Create uORB::PublicationMulti subscriptions_multi: diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index ac96b8586faf..5ecde816d139 100644 --- a/src/modules/zenoh/Kconfig.topics +++ b/src/modules/zenoh/Kconfig.topics @@ -445,10 +445,6 @@ menu "Zenoh publishers/subscribers" bool "normalized_unsigned_setpoint" default n - config ZENOH_PUBSUB_NPFG_STATUS - bool "npfg_status" - default n - config ZENOH_PUBSUB_OBSTACLE_DISTANCE bool "obstacle_distance" default n