diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 81af5e1acd..b6f05837be 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -16,6 +16,7 @@ control_allocator start fw_rate_control start fw_att_control start fw_pos_control 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 c28adef56a..9962f3c83a 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_pos_control 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 549ba99b73..844782f2af 100644 --- a/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board +++ b/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board @@ -42,6 +42,7 @@ 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_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 33ad75b695..e34edeb0b7 100644 --- a/boards/airmind/mindpx-v2/default.px4board +++ b/boards/airmind/mindpx-v2/default.px4board @@ -47,6 +47,7 @@ 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_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 0ec7b49c42..6b767e4675 100644 --- a/boards/ark/fmu-v6x/default.px4board +++ b/boards/ark/fmu-v6x/default.px4board @@ -57,6 +57,7 @@ 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_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 f1f021b44e..ef0766f144 100644 --- a/boards/ark/fpv/default.px4board +++ b/boards/ark/fpv/default.px4board @@ -38,6 +38,7 @@ 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_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/av/x-v1/default.px4board b/boards/av/x-v1/default.px4board index 86db7c85a3..a493a5623c 100644 --- a/boards/av/x-v1/default.px4board +++ b/boards/av/x-v1/default.px4board @@ -43,6 +43,7 @@ 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_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 79e0d80b92..5a2f5682aa 100644 --- a/boards/beaglebone/blue/default.px4board +++ b/boards/beaglebone/blue/default.px4board @@ -31,6 +31,7 @@ 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_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/cuav/7-nano/default.px4board b/boards/cuav/7-nano/default.px4board index f6e4511ae2..fc50c0ba7a 100644 --- a/boards/cuav/7-nano/default.px4board +++ b/boards/cuav/7-nano/default.px4board @@ -43,6 +43,7 @@ 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_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 675ffa54cb..b06d1005b3 100644 --- a/boards/cuav/nora/default.px4board +++ b/boards/cuav/nora/default.px4board @@ -51,6 +51,7 @@ 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_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 c7c30b1a1e..5491c10fc1 100644 --- a/boards/cuav/x7pro/default.px4board +++ b/boards/cuav/x7pro/default.px4board @@ -51,6 +51,7 @@ 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_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 9bb4f368f9..411407613a 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -50,6 +50,7 @@ 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_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 ba6ed0b34f..416ccf5483 100644 --- a/boards/cubepilot/cubeorangeplus/default.px4board +++ b/boards/cubepilot/cubeorangeplus/default.px4board @@ -51,6 +51,7 @@ 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_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 62cf898af9..3717771be6 100644 --- a/boards/cubepilot/cubeyellow/default.px4board +++ b/boards/cubepilot/cubeyellow/default.px4board @@ -47,6 +47,7 @@ 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_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 722f42b410..11a066013a 100644 --- a/boards/emlid/navio2/default.px4board +++ b/boards/emlid/navio2/default.px4board @@ -33,6 +33,7 @@ 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_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 4327646414..18a7d92520 100644 --- a/boards/hkust/nxt-dual/default.px4board +++ b/boards/hkust/nxt-dual/default.px4board @@ -40,6 +40,7 @@ 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_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 b721607758..914bfd3b3a 100644 --- a/boards/hkust/nxt-v1/default.px4board +++ b/boards/hkust/nxt-v1/default.px4board @@ -42,6 +42,7 @@ 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_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 ecc1bc8101..199bc05367 100644 --- a/boards/holybro/durandal-v1/default.px4board +++ b/boards/holybro/durandal-v1/default.px4board @@ -45,6 +45,7 @@ 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_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 f4a8e0c824..cdebf407b2 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -46,6 +46,7 @@ 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_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 c023eae04f..b8ca1495a2 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -46,6 +46,7 @@ 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_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 0f1b80b250..8bbb8f6a7e 100644 --- a/boards/holybro/kakuteh7v2/default.px4board +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -46,6 +46,7 @@ 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_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 0093ee2dcf..94b177fb87 100644 --- a/boards/holybro/pix32v5/default.px4board +++ b/boards/holybro/pix32v5/default.px4board @@ -51,6 +51,7 @@ 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_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 22efafcfdb..26238797e0 100644 --- a/boards/matek/h743-mini/default.px4board +++ b/boards/matek/h743-mini/default.px4board @@ -31,6 +31,7 @@ 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_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 48e937fc43..b4fe01dbdf 100644 --- a/boards/matek/h743-slim/default.px4board +++ b/boards/matek/h743-slim/default.px4board @@ -34,6 +34,7 @@ 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_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 71024fea5c..c530e5303d 100644 --- a/boards/matek/h743/default.px4board +++ b/boards/matek/h743/default.px4board @@ -33,6 +33,7 @@ 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_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 2d92e89d96..7af585274e 100644 --- a/boards/micoair/h743-aio/default.px4board +++ b/boards/micoair/h743-aio/default.px4board @@ -40,6 +40,7 @@ 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_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 74f1e2de29..518132aa8d 100644 --- a/boards/micoair/h743-v2/default.px4board +++ b/boards/micoair/h743-v2/default.px4board @@ -41,6 +41,7 @@ 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_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 149bcaff21..207bb39bbb 100644 --- a/boards/micoair/h743/default.px4board +++ b/boards/micoair/h743/default.px4board @@ -39,6 +39,7 @@ 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_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 b73fdd18f8..7f60b557f2 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -50,6 +50,7 @@ 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_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 f95c859e8c..b97c988067 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -41,6 +41,7 @@ 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_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 665fbc8626..86cadee29a 100644 --- a/boards/mro/ctrl-zero-classic/default.px4board +++ b/boards/mro/ctrl-zero-classic/default.px4board @@ -47,6 +47,7 @@ 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_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 9fe340b87f..13ee784818 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.px4board +++ b/boards/mro/ctrl-zero-f7-oem/default.px4board @@ -45,6 +45,7 @@ CONFIG_MODULES_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_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 d09ca8a5b5..e9ad953550 100644 --- a/boards/mro/ctrl-zero-f7/default.px4board +++ b/boards/mro/ctrl-zero-f7/default.px4board @@ -45,6 +45,7 @@ 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_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 ec6c59888d..643c4f1a8a 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.px4board +++ b/boards/mro/ctrl-zero-h7-oem/default.px4board @@ -43,6 +43,7 @@ CONFIG_MODULES_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_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 7bb96d3aca..368709da9b 100644 --- a/boards/mro/ctrl-zero-h7/default.px4board +++ b/boards/mro/ctrl-zero-h7/default.px4board @@ -44,6 +44,7 @@ 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_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 fb49c90866..46d5ac549b 100644 --- a/boards/mro/pixracerpro/default.px4board +++ b/boards/mro/pixracerpro/default.px4board @@ -44,6 +44,7 @@ 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_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 cec5dd074a..c5c5f82b98 100644 --- a/boards/mro/x21-777/default.px4board +++ b/boards/mro/x21-777/default.px4board @@ -46,6 +46,7 @@ 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_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 8640bff623..dd3a6b85f2 100644 --- a/boards/mro/x21/default.px4board +++ b/boards/mro/x21/default.px4board @@ -47,6 +47,7 @@ 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_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 749e62e5d5..d140a687d5 100644 --- a/boards/nxp/fmuk66-e/default.px4board +++ b/boards/nxp/fmuk66-e/default.px4board @@ -48,6 +48,7 @@ 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_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 268e89c70f..40ca30a25e 100644 --- a/boards/nxp/fmuk66-v3/default.px4board +++ b/boards/nxp/fmuk66-v3/default.px4board @@ -49,6 +49,7 @@ 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_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 48a7883fe2..c681da011b 100644 --- a/boards/nxp/mr-canhubk3/fmu.px4board +++ b/boards/nxp/mr-canhubk3/fmu.px4board @@ -34,6 +34,7 @@ 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_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 2089b09931..cf687fcfb9 100644 --- a/boards/nxp/mr-canhubk3/sysview.px4board +++ b/boards/nxp/mr-canhubk3/sysview.px4board @@ -21,6 +21,7 @@ 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_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 fe2297a554..8d4876a99f 100644 --- a/boards/nxp/mr-canhubk3/zenoh.px4board +++ b/boards/nxp/mr-canhubk3/zenoh.px4board @@ -25,6 +25,7 @@ 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_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 c7fcb205e0..c1beb1fafd 100644 --- a/boards/nxp/tropic-community/default.px4board +++ b/boards/nxp/tropic-community/default.px4board @@ -48,6 +48,7 @@ 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_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 0bde0c7e07..3449d56615 100644 --- a/boards/px4/fmu-v2/fixedwing.px4board +++ b/boards/px4/fmu-v2/fixedwing.px4board @@ -9,4 +9,5 @@ CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL=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 5dec8fda22..bd268cbe2f 100644 --- a/boards/px4/fmu-v3/default.px4board +++ b/boards/px4/fmu-v3/default.px4board @@ -52,6 +52,7 @@ 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_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 e7906db52c..bec469793e 100644 --- a/boards/px4/fmu-v4/default.px4board +++ b/boards/px4/fmu-v4/default.px4board @@ -53,6 +53,7 @@ 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_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 ff8455c285..d090ffb140 100644 --- a/boards/px4/fmu-v4pro/default.px4board +++ b/boards/px4/fmu-v4pro/default.px4board @@ -48,6 +48,7 @@ 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_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 1132a48bbf..13bcff46a6 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -56,6 +56,7 @@ 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_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 7ea307124c..99c5c497f6 100644 --- a/boards/px4/fmu-v5/protected.px4board +++ b/boards/px4/fmu-v5/protected.px4board @@ -18,6 +18,7 @@ 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_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 2c9e2d20d3..5fa964bc3a 100644 --- a/boards/px4/fmu-v5/rover.px4board +++ b/boards/px4/fmu-v5/rover.px4board @@ -5,6 +5,7 @@ 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_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 e8bcadebd7..b182a1c83a 100644 --- a/boards/px4/fmu-v5/stackcheck.px4board +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -30,6 +30,7 @@ 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_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 8ba41726c0..9115b13ad7 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -60,6 +60,7 @@ 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_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 42f8ec21ce..2490c28c6b 100644 --- a/boards/px4/fmu-v5x/rover.px4board +++ b/boards/px4/fmu-v5x/rover.px4board @@ -4,6 +4,7 @@ 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_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 e6b812fffd..e32ec2e54b 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -48,6 +48,7 @@ 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_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 9eed13c665..ee7cddbe05 100644 --- a/boards/px4/fmu-v6c/rover.px4board +++ b/boards/px4/fmu-v6c/rover.px4board @@ -3,6 +3,7 @@ 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_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 be90aca727..55d857d697 100644 --- a/boards/px4/fmu-v6u/default.px4board +++ b/boards/px4/fmu-v6u/default.px4board @@ -48,6 +48,7 @@ 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_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 9eed13c665..ee7cddbe05 100644 --- a/boards/px4/fmu-v6u/rover.px4board +++ b/boards/px4/fmu-v6u/rover.px4board @@ -3,6 +3,7 @@ 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_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 ba42e02a9d..901318cf30 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -58,6 +58,7 @@ 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_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 9f540f567d..b501d6fe4a 100644 --- a/boards/px4/fmu-v6x/multicopter.px4board +++ b/boards/px4/fmu-v6x/multicopter.px4board @@ -4,6 +4,7 @@ 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_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 9eed13c665..ee7cddbe05 100644 --- a/boards/px4/fmu-v6x/rover.px4board +++ b/boards/px4/fmu-v6x/rover.px4board @@ -3,6 +3,7 @@ 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_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 477ba76ed8..6710e97b6a 100644 --- a/boards/px4/fmu-v6xrt/default.px4board +++ b/boards/px4/fmu-v6xrt/default.px4board @@ -58,6 +58,7 @@ 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_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 7b8ed3bd1f..f7af280b03 100644 --- a/boards/px4/fmu-v6xrt/rover.px4board +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -4,6 +4,7 @@ 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_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 ade7dd4235..c8034269aa 100644 --- a/boards/px4/raspberrypi/default.px4board +++ b/boards/px4/raspberrypi/default.px4board @@ -31,6 +31,7 @@ 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_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 f85c03e23a..7da14ff720 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -20,6 +20,7 @@ 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_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 56c966598b..121c7ca9f7 100644 --- a/boards/px4/sitl/spacecraft.px4board +++ b/boards/px4/sitl/spacecraft.px4board @@ -3,6 +3,7 @@ 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_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 b72da494da..8e18d35c46 100644 --- a/boards/scumaker/pilotpi/default.px4board +++ b/boards/scumaker/pilotpi/default.px4board @@ -33,6 +33,7 @@ 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_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 2a7a232581..a532e87e99 100644 --- a/boards/siyi/n7/default.px4board +++ b/boards/siyi/n7/default.px4board @@ -39,6 +39,7 @@ 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_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 fdfdcdea6a..7b2a0d7a90 100644 --- a/boards/sky-drones/smartap-airlink/default.px4board +++ b/boards/sky-drones/smartap-airlink/default.px4board @@ -47,6 +47,7 @@ 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_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 d760279546..448268f416 100644 --- a/boards/thepeach/k1/default.px4board +++ b/boards/thepeach/k1/default.px4board @@ -45,6 +45,7 @@ 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_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 d760279546..448268f416 100644 --- a/boards/thepeach/r1/default.px4board +++ b/boards/thepeach/r1/default.px4board @@ -45,6 +45,7 @@ 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_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 09a32b78c5..ccb5fe5396 100644 --- a/boards/x-mav/ap-h743v2/default.px4board +++ b/boards/x-mav/ap-h743v2/default.px4board @@ -41,6 +41,7 @@ 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_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 ba05f4ab5f..85b3a9e993 100644 --- a/boards/zeroone/x6/default.px4board +++ b/boards/zeroone/x6/default.px4board @@ -48,6 +48,7 @@ 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_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 6c374f2c35..e3008ae917 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -90,6 +90,10 @@ set(msg_files FollowTargetEstimator.msg FollowTargetStatus.msg FuelTankStatus.msg + FixedWingLateralSetpoint.msg + FixedWingLongitudinalSetpoint.msg + FixedWingLateralGuidanceStatus.msg + FixedWingLateralStatus.msg GeneratorStatus.msg GeofenceResult.msg GeofenceStatus.msg @@ -120,10 +124,12 @@ set(msg_files LandingGearWheel.msg LandingTargetInnovations.msg LandingTargetPose.msg + LateralControlConfiguration.msg LaunchDetectionStatus.msg LedControl.msg LoggerStatus.msg LogMessage.msg + LongitudinalControlConfiguration.msg MagnetometerBiasEstimate.msg MagWorkerData.msg ManualControlSwitches.msg @@ -137,7 +143,6 @@ set(msg_files NavigatorMissionItem.msg NavigatorStatus.msg NormalizedUnsignedSetpoint.msg - NpfgStatus.msg ObstacleDistance.msg OffboardControlMode.msg OnboardComputerStatus.msg diff --git a/msg/FixedWingLateralGuidanceStatus.msg b/msg/FixedWingLateralGuidanceStatus.msg new file mode 100644 index 0000000000..6295f8948e --- /dev/null +++ b/msg/FixedWingLateralGuidanceStatus.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) + +float32 course_setpoint # bearing angle (same as course) [rad] +float32 lateral_acceleration_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 adapted_period # adapted period (if auto-tuning enabled) [s] +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) diff --git a/msg/FixedWingLateralSetpoint.msg b/msg/FixedWingLateralSetpoint.msg new file mode 100644 index 0000000000..d899993ad5 --- /dev/null +++ b/msg/FixedWingLateralSetpoint.msg @@ -0,0 +1,7 @@ +uint64 timestamp + +# NOTE: At least one of course, airspeed_direction, or lateral_acceleration must be finite + +float32 course # [-pi, pi] Course over ground setpoint, w.r.t. North. NAN if not controlled directly. +float32 airspeed_direction # [-pi, pi] Angle projected to ground of desired airspeed vector, w.r.t. North. NAN if not controlled directly, used as feedforward if course setpoint is finite. +float32 lateral_acceleration # [m/s^2] Lateral acceleration setpoint in FRD frame. NAN if not controlled directly, used as feedforward if either course setpoint or airspeed_direction is finite. diff --git a/msg/FixedWingLateralStatus.msg b/msg/FixedWingLateralStatus.msg new file mode 100644 index 0000000000..b877948b24 --- /dev/null +++ b/msg/FixedWingLateralStatus.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +float32 lateral_acceleration # resultant lateral acceleration reference [m/s^2] +float32 can_run_factor # estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1] diff --git a/msg/FixedWingLongitudinalSetpoint.msg b/msg/FixedWingLongitudinalSetpoint.msg new file mode 100644 index 0000000000..25595f76c4 --- /dev/null +++ b/msg/FixedWingLongitudinalSetpoint.msg @@ -0,0 +1,9 @@ +uint64 timestamp + +# Note: If not both pitch_direct and throttle_direct are finite, then either altitude or height_rate must be finite + +float32 altitude # [m] Altitude setpoint AMSL, NAN if not controlled +float32 height_rate # [m/s] NAN if not controlled directly, used as feedforward if altitude is finite +float32 equivalent_airspeed # [m/s] NAN if system default should be used +float32 pitch_direct # [rad] NAN if not controlled, overrides total energy controller +float32 throttle_direct # [0,1] NAN if not controlled, overrides total energy controller diff --git a/msg/LateralControlConfiguration.msg b/msg/LateralControlConfiguration.msg new file mode 100644 index 0000000000..ee96028776 --- /dev/null +++ b/msg/LateralControlConfiguration.msg @@ -0,0 +1,3 @@ +uint64 timestamp + +float32 lateral_accel_max # [m/s^2] maps 1:1 to a maximum roll angle, accel_max = tan(roll_max) * GRAVITY diff --git a/msg/LongitudinalControlConfiguration.msg b/msg/LongitudinalControlConfiguration.msg new file mode 100644 index 0000000000..0f944cc9bf --- /dev/null +++ b/msg/LongitudinalControlConfiguration.msg @@ -0,0 +1,11 @@ +uint64 timestamp + +float32 pitch_min # [rad] +float32 pitch_max # [rad] +float32 throttle_min # [0,1] +float32 throttle_max # [0,1] +float32 climb_rate_target # [m/s] target climbrate used to change altitude +float32 sink_rate_target # [m/s] target sinkrate used to change altitude +float32 speed_weight # [0,2], 0=pitch controls altitude only, 2=pitch controls airspeed only +bool enforce_low_height_condition # if true, total energy controller will use lower altitude control time constant +bool disable_underspeed_protection # if true, underspeed handling is disabled in the total energy controller diff --git a/msg/NpfgStatus.msg b/msg/NpfgStatus.msg deleted file mode 100644 index 132c1f7f3f..0000000000 --- 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/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index b452595732..c61c43c0e2 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_pos_control start +fw_lat_lon_control start fw_att_control start vtol airspeed_selector start diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index d6ba875fde..cf8bd9fef6 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -57,6 +57,7 @@ land_detector start fixedwing fw_att_control start fw_pos_control 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/px4_fw.config b/posix-configs/rpi/px4_fw.config index 55d8d9cfd9..74f59a3eb1 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -42,6 +42,7 @@ ekf2 start land_detector start fixedwing fw_att_control start fw_pos_control start +fw_lat_lon_control start mavlink start -x -u 14556 -r 1000000 -p diff --git a/src/lib/npfg/AirspeedDirectionController.cpp b/src/lib/npfg/AirspeedDirectionController.cpp new file mode 100644 index 0000000000..621587f47e --- /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 0000000000..d0111729ca --- /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 739e6af155..d5da8fb2f2 100644 --- a/src/lib/npfg/CMakeLists.txt +++ b/src/lib/npfg/CMakeLists.txt @@ -32,8 +32,12 @@ ############################################################################ 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) diff --git a/src/lib/npfg/CourseToAirspeedRefMapper.cpp b/src/lib/npfg/CourseToAirspeedRefMapper.cpp new file mode 100644 index 0000000000..7e745486b4 --- /dev/null +++ b/src/lib/npfg/CourseToAirspeedRefMapper.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 airsp_dot_bearing = projectAirspOnBearing(airspeed_sp, wind_cross_bearing); + const Vector2f air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vector); + + // TODO check if we need to use infeasibleAirVelRef or other mitigation functions in some cases (high wind) + + 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 0000000000..70f3312e9b --- /dev/null +++ b/src/lib/npfg/CourseToAirspeedRefMapper.hpp @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * 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: + /* + * 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_setpoint Bearing + * @param[in] airspeed_true True airspeed setpoint[m/s] + * @return Air velocity vector [m/s] + */ + matrix::Vector2f refAirVelocity(const matrix::Vector2f &wind_vel, const float bearing_setpoint, + float airspeed_true, float min_ground_speed) const; + /* + * 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 0000000000..cd1f7aef93 --- /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 c36184ef96..691b1fd4dd 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 @@ public: */ 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 @@ public: 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 @@ public: */ 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 @@ private: // 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 @@ private: 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 @@ private: // 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 + * 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 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 - */ - - 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 @@ private: 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 @@ private: */ 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 @@ private: * @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 @@ private: 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 @@ private: * @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 @@ private: 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/npfg.cpp b/src/lib/npfg/npfg.cpp deleted file mode 100644 index fdcfa570b0..0000000000 --- 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/modules/fw_lateral_longitudinal_control/CMakeLists.txt b/src/modules/fw_lateral_longitudinal_control/CMakeLists.txt new file mode 100644 index 0000000000..7642594b7a --- /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 0000000000..eb92670ffa --- /dev/null +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -0,0 +1,841 @@ +/**************************************************************************** + * + * 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()); + _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(); + 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 = 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()); + + 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 0000000000..703fb722e9 --- /dev/null +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp @@ -0,0 +1,260 @@ +/**************************************************************************** + * + * 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 + +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 _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{}; + + 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 0000000000..aefa5a7559 --- /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 0000000000..130c0a1bd7 --- /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 Path 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 TECS + */ +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 TECS + */ +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 TECS + */ +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 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); + +/** + * 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); + +/** + * 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); + +/** + * 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); + +/** + * 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); diff --git a/src/modules/fw_pos_control/CMakeLists.txt b/src/modules/fw_pos_control/CMakeLists.txt index 9248e8d2df..163cf17e67 100644 --- a/src/modules/fw_pos_control/CMakeLists.txt +++ b/src/modules/fw_pos_control/CMakeLists.txt @@ -60,6 +60,8 @@ px4_add_module( SRCS FixedwingPositionControl.cpp FixedwingPositionControl.hpp + ControllerConfigurationHandler.cpp + ControllerConfigurationHandler.hpp DEPENDS ${POSCONTROL_DEPENDENCIES} ) diff --git a/src/modules/fw_pos_control/ControllerConfigurationHandler.cpp b/src/modules/fw_pos_control/ControllerConfigurationHandler.cpp new file mode 100644 index 0000000000..fcfabb1679 --- /dev/null +++ b/src/modules/fw_pos_control/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_pos_control/ControllerConfigurationHandler.hpp b/src/modules/fw_pos_control/ControllerConfigurationHandler.hpp new file mode 100644 index 0000000000..73cfc78e4b --- /dev/null +++ b/src/modules/fw_pos_control/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_pos_control/FixedwingPositionControl.cpp index de00c74b59..328a556be2 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.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 @@ -34,6 +34,7 @@ #include "FixedwingPositionControl.hpp" #include +#include using math::constrain; using math::max; @@ -48,41 +49,30 @@ 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}; + +FixedwingPositionControl::FixedwingPositionControl() : 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(); - _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() @@ -106,51 +96,13 @@ FixedwingPositionControl::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 @@ -207,45 +159,25 @@ FixedwingPositionControl::vehicle_command_poll() void FixedwingPositionControl::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() +FixedwingPositionControl::wind_poll(const hrt_abstime now) { if (_wind_sub.updated()) { wind_s wind; @@ -255,14 +187,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) { @@ -294,18 +226,17 @@ FixedwingPositionControl::manual_control_setpoint_poll() } } - void FixedwingPositionControl::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,180 +251,33 @@ 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() { - - 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(); -} - -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); + return manual_airspeed_setpoint; } void @@ -509,47 +293,6 @@ 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) { @@ -591,76 +334,18 @@ 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() { - /* - * 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; @@ -670,7 +355,7 @@ void FixedwingPositionControl::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; @@ -687,7 +372,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 +412,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 +443,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 +455,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 +464,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 +473,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; @@ -821,7 +498,6 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now) if (_landed) { _completed_manual_takeoff = false; } - } void @@ -847,7 +523,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); @@ -886,13 +561,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 +576,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,90 +599,89 @@ 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 -FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval) +void FixedwingPositionControl::control_idle() { - const bool is_low_height = checkLowHeightConditions(); + 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); - // 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); + 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); - const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle - const float yaw_body = 0.f; + _ctrl_configuration_handler.setThrottleMax(0.0f); + _ctrl_configuration_handler.setThrottleMin(0.0f); +} + +void +FixedwingPositionControl::control_auto_fixed_bank_alt_hold() +{ + 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 + }; + + _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) +FixedwingPositionControl::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 float descend_rate = 0.5f; - const bool is_low_height = checkLowHeightConditions(); + const hrt_abstime now = hrt_absolute_time(); - 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 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 + }; + _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 - const float yaw_body = 0.f; - - // 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 float pitch_body = get_tecs_pitch(); - const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); + lateral_ctrl_sp.lateral_acceleration = rollAngleToLateralAccel(roll_body); + _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); } uint8_t @@ -1031,13 +699,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) { @@ -1072,20 +739,8 @@ void FixedwingPositionControl::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 +777,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, 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 position_sp_alt = pos_sp_curr.alt; - - //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 float target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0))); - float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); + const Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + const DirectionalGuidanceOutput sp = navigateBearing(curr_pos_local, target_bearing, ground_speed, _wind_vel); - 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; + 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); - float yaw_body = _yaw; - const bool disable_underspeed_handling = false; + const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN; - const bool is_low_height = checkLowHeightConditions(); + 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 + }; - 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(); + _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::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, + 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 +876,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); - const bool close_to_circle = vehicle_to_loiter_center.norm() < loiter_radius + _npfg.switchDistance(500); + bool enforce_low_height{false}; + + 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); + const DirectionalGuidanceOutput sp = navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, + pos_sp_curr.loiter_direction_counter_clockwise, + ground_speed, + _wind_vel); - _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; + 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 yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - - 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 +918,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 + }; - 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); + } + + _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) + 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,47 +969,33 @@ 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(); + const DirectionalGuidanceOutput sp = _figure_eight.updateSetpoint(curr_pos_local, ground_speed, params); + + 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); + _closest_point_on_path = _figure_eight.getClosestPoint(); - // TECS - float tecs_fw_thr_min; - float tecs_fw_thr_max; + 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); 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(); + _ctrl_configuration_handler.setThrottleMin(0.0f); + _ctrl_configuration_handler.setThrottleMax(0.0f); + _ctrl_configuration_handler.setSpeedWeight(2.0f); } - - 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); - - // Yaw - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - 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); } void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_s pos_sp) @@ -1432,60 +1018,41 @@ void FixedwingPositionControl::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); + const DirectionalGuidanceOutput sp = navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), + ground_speed, _wind_vel, curvature); - float roll_body = getCorrectedNpfgRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + 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); - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + 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 + }; - const bool is_low_height = checkLowHeightConditions(); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - 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 @@ -1504,27 +1071,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 +1094,12 @@ 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 +1114,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); + const DirectionalGuidanceOutput sp = navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, + _wind_vel); - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(start_pos_local, 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; - float roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint()); + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + const float roll_wingtip_strike = getMaxRollAngleNearGround(_current_altitude, _takeoff_ground_alt); + _ctrl_configuration_handler.setLateralAccelMax(rollAngleToLateralAccel(roll_wingtip_strike)); - // use npfg's bearing to commanded course, controlled via yaw angle while on runway - const float bearing = _npfg.getBearing(); - - // heading hold mode will override this bearing setpoint - float yaw_body = _runway_takeoff.getYaw(bearing); - - // 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; + 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()) + }; - // throttle is open loop anyway during ground roll, no need to wind up the integrator - _tecs.resetIntegrals(); - } + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - 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()); - - roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); - - 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(); @@ -1646,14 +1173,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 +1199,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; + _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 + }; - 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(); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - } else { - _att_sp.thrust_body[0] = get_tecs_thrust(); - } + _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); - float pitch_body = get_tecs_pitch(); - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - - 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); + //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; @@ -1746,20 +1264,10 @@ void FixedwingPositionControl::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 +1315,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 +1325,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,22 +1335,19 @@ 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 */ @@ -1877,47 +1379,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); + 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 + }; - /* set the attitude and throttle commands */ + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - // 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); - - // 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; + _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,51 +1405,41 @@ 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()); + const float desired_max_sinkrate = math::max(glide_slope_sink_rate, _param_sinkrate_target.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); + 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 + }; - /* set the attitude and throttle commands */ + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - float pitch_body = get_tecs_pitch(); - - roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); - - // 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 - _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -1989,19 +1457,10 @@ void FixedwingPositionControl::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()); + _param_fw_airspd_min.get(); - 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); + _ctrl_configuration_handler.setEnforceLowHeightCondition(true); const Vector2f local_position{_local_pos.x, _local_pos.y}; @@ -2026,14 +1485,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 +1493,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 +1503,15 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, 1.0f); /* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */ + 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; - // 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 - + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); /* longitudinal guidance */ const float flare_ramp_interpolator_sqrt = sqrtf(flare_ramp_interpolator); @@ -2099,99 +1541,62 @@ 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; + 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 + }; - 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); + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - /* 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; + _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; + const float desired_max_sinkrate = math::max(glide_slope_sink_rate, _param_sinkrate_target.get()); - 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 + 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 + }; - /* set the attitude and throttle commands */ + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - pitch_body = get_tecs_pitch(); - - // yaw is not controlled in nominal flight - yaw_body = _yaw; - - // 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); - - Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); - attitude_setpoint.copyTo(_att_sp.q_d); - + _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(); @@ -2210,66 +1615,58 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, { 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, +FixedwingPositionControl::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 +1674,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 +1698,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 +1710,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 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 disable_underspeed_handling = false; + _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,16 +1745,17 @@ 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 FixedwingPositionControl::rollAngleToLateralAccel(float roll_body) const +{ + return tanf(roll_body) * CONSTANTS_ONE_G; } void FixedwingPositionControl::control_backtransition_heading_hold() @@ -2374,34 +1764,10 @@ void FixedwingPositionControl::control_backtransition_heading_hold() _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, @@ -2410,47 +1776,23 @@ void FixedwingPositionControl::control_backtransition_line_follow(const Vector2f 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(); + DirectionalGuidanceOutput sp = {.course_setpoint = NAN, .lateral_acceleration_feedforward = 0.f}; - 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()); + if (_control_mode.flag_control_position_enabled) { + sp = navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } - // return level flight pitch offset to prevent stale tecs state when it's not running - return radians(_param_fw_psp_off.get()); -} - -float -FixedwingPositionControl::get_tecs_thrust() -{ - if (_tecs_is_running) { - return min(_tecs.get_throttle_setpoint(), 1.f); - } - - // 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 @@ -2464,13 +1806,30 @@ FixedwingPositionControl::Run() perf_begin(_loop_perf); - /* only run controller if position changed */ + if (_vehicle_status_sub.updated()) { - if (_local_pos_sub.update(&_local_pos)) { + if (_vehicle_status_sub.update(&_vehicle_status)) { + _nav_state = _vehicle_status.nav_state; + } + } - const float control_interval = math::constrain((_local_pos.timestamp - _last_time_position_control_called) * 1e-6f, + /* 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 (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(); + + } 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 +1859,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 +1966,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,38 +1976,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; + // by default set speed weight to the param value, can be overwritten inside the methods below + _ctrl_configuration_handler.setSpeedWeight(_param_t_spdweight.get()); // 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; @@ -2686,23 +2025,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 +2051,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 +2066,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 +2080,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 +2097,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 +2106,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); @@ -2844,86 +2145,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 FixedwingPositionControl::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,11 +2162,12 @@ 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, const float land_point_altitude, const Vector2f &local_position, const Vector2f &local_land_point) @@ -2997,27 +2227,6 @@ 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) { @@ -3030,8 +2239,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(); @@ -3137,9 +2346,6 @@ 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); } @@ -3162,7 +2368,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 FixedwingPositionControl::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 +2379,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 +2394,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 FixedwingPositionControl::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 FixedwingPositionControl::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 +2439,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 FixedwingPositionControl::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 +2456,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 FixedwingPositionControl::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 +2496,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 FixedwingPositionControl::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 FixedwingPositionControl::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); +} + +void FixedwingPositionControl::publish_lateral_guidance_status(const hrt_abstime now) +{ + fixed_wing_lateral_guidance_status_s fixed_wing_lateral_guidance_status{}; + + 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); } int FixedwingPositionControl::task_spawn(int argc, char *argv[]) { - bool vtol = false; - - if (argc > 1) { - if (strcmp(argv[1], "vtol") == 0) { - vtol = true; - } - } - - FixedwingPositionControl *instance = new FixedwingPositionControl(vtol); + FixedwingPositionControl *instance = new FixedwingPositionControl(); if (instance) { _object.store(instance); @@ -3370,12 +2587,11 @@ fw_pos_control is the fixed-wing position controller. PRINT_MODULE_USAGE_NAME("fw_pos_control", "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 FixedwingPositionControl::getLoadFactor() const { float load_factor_from_bank_angle = 1.0f; @@ -3389,7 +2605,6 @@ float FixedwingPositionControl::getLoadFactor() } - extern "C" __EXPORT int fw_pos_control_main(int argc, char *argv[]) { return FixedwingPositionControl::main(argc, argv); diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index e56263c5e3..e7526dca40 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.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 @@ -35,13 +35,6 @@ /** * @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 */ #ifndef FIXEDWINGPOSITIONCONTROL_HPP_ @@ -49,15 +42,13 @@ #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,25 @@ #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 +89,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 +105,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 +122,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,23 +147,14 @@ 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; - -// [-] 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; +// [] Stick deadzon +static constexpr float kStickDeadBand = 0.06f; class FixedwingPositionControl final : public ModuleBase, public ModuleParams, public px4::WorkItem { public: - FixedwingPositionControl(bool vtol = false); + FixedwingPositionControl(); ~FixedwingPositionControl() override; /** @see ModuleBase */ @@ -210,33 +182,31 @@ private: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; - uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Publication _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)}; 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 +242,12 @@ private: // VEHICLE STATES + uint8_t _nav_state; + double _current_latitude{0}; double _current_longitude{0}; float _current_altitude{0.f}; - float _roll{0.f}; - float _pitch{0.0f}; float _yaw{0.0f}; float _yawrate{0.0f}; @@ -323,8 +293,8 @@ private: // true if a launch, specifically using the launch detector, has been detected bool _launch_detected{false}; - // [deg] global position of the vehicle at the time launch is detected (using launch detector) - Vector2d _launch_global_position{0, 0}; + // [deg] global position of the vehicle at the time launch is detected (using launch detector) or takeoff is started (runway) + Vector2d _takeoff_init_position{0, 0}; // [rad] current vehicle yaw at the time the launch is detected float _launch_current_yaw{0.f}; @@ -363,7 +333,6 @@ private: bool flaring{false}; hrt_abstime start_time{0}; // [us] float initial_height_rate_setpoint{0.0f}; // [m/s] - float initial_throttle_setpoint{0.0f}; } _flare_states; // [m] last terrain estimate which was valid @@ -381,9 +350,7 @@ private: // AIRSPEED float _airspeed_eas{0.f}; - float _eas2tas{1.f}; bool _airspeed_valid{false}; - float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos}; // [us] last time airspeed was received. used to detect timeouts. hrt_abstime _time_airspeed_last_valid{0}; @@ -397,24 +364,12 @@ private: hrt_abstime _time_wind_last_received{0}; // [us] - // TECS - - // total energy control system - airspeed / altitude control - TECS _tecs; - - bool _tecs_is_running{false}; - - // Smooths changes in the altitude tracking error time constant value - SlewRate _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 +378,7 @@ private: matrix::Vector2f _closest_point_on_path; // nonlinear path following guidance - lateral-directional position control - NPFG _npfg; - 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 - - PerformanceModel _performance_model; + DirectionalGuidance _directional_guidance; // LANDING GEAR int8_t _new_landing_gear_position{landing_gear_s::GEAR_KEEP}; @@ -439,7 +389,6 @@ private: hrt_abstime _time_in_fixed_bank_loiter{0}; // [us] float _min_current_sp_distance_xy{FLT_MAX}; - float _target_bearing{0.0f}; // [rad] #ifdef CONFIG_FIGURE_OF_EIGHT /* Loitering */ @@ -451,11 +400,10 @@ private: * @param control_interval Time since last position control call [s] * @param curr_pos the current 2D absolute position of the vehicle in [deg]. * @param ground_speed the 2D ground speed of the vehicle in [m/s]. - * @param pos_sp_prev the previous position setpoint. * @param pos_sp_curr the current position setpoint. */ void 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); + const position_setpoint_s &pos_sp_curr); void publishFigureEightStatus(const position_setpoint_s pos_sp); #endif // CONFIG_FIGURE_OF_EIGHT @@ -465,27 +413,18 @@ private: // Update subscriptions void airspeed_poll(); - void control_update(); + void manual_control_setpoint_poll(); void vehicle_attitude_poll(); void vehicle_command_poll(); void vehicle_control_mode_poll(); - void vehicle_status_poll(); - void wind_poll(); - void status_publish(); + void wind_poll(const hrt_abstime now); + void landing_status_publish(); - void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw, - float throttle_trim); - void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint); - float getLoadFactor(); - /** - * @brief Get the NPFG roll setpoint with mitigation strategy if npfg is not certain about its output - * - * @return roll setpoint - */ - float getCorrectedNpfgRollSetpoint(); + void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint); + float getLoadFactor() const; /** * @brief Sets the landing abort status and publishes landing status. @@ -503,24 +442,6 @@ private: */ bool checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask, uint8_t landing_abort_criterion); - /** - * @brief Get a new waypoint based on heading and distance from current position - * - * @param heading the heading to fly to - * @param distance the distance of the generated waypoint - * @param waypoint_prev the waypoint at the current position - * @param waypoint_next the waypoint in the heading direction - */ - void get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev, - position_setpoint_s &waypoint_next, bool flag_init); - - /** - * @brief Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available - * - * @param takeoff_alt Altitude AMSL at launch or when runway takeoff is detected [m] - */ - float get_terrain_altitude_takeoff(float takeoff_alt); - /** * @brief Maps the manual control setpoint (pilot sticks) to height rate commands * @@ -535,13 +456,6 @@ private: */ void updateManualTakeoffStatus(); - /** - * @brief Update desired altitude base on user pitch stick input - * - * @param dt Time step - */ - void update_desired_altitude(float dt); - /** * @brief Updates timing information for landed and in-air states. * @@ -585,19 +499,15 @@ private: * @brief Controls altitude and airspeed for a fixed-bank loiter. * * Used as a failsafe mode after a lateral position estimate failure. - * - * @param control_interval Time since last position control call [s] */ - void control_auto_fixed_bank_alt_hold(const float control_interval); + void control_auto_fixed_bank_alt_hold(); /** * @brief Control airspeed with a fixed descent rate and roll angle. * * Used as a failsafe mode after a lateral position estimate failure. - * - * @param control_interval Time since last position control call [s] */ - void control_auto_descend(const float control_interval); + void control_auto_descend(); /** * @brief Vehicle control for position waypoints. @@ -617,12 +527,11 @@ private: * @param control_interval Time since last position control call [s] * @param curr_pos Current 2D local position vector of vehicle [m] * @param ground_speed Local 2D ground speed of vehicle [m/s] - * @param pos_sp_prev previous position setpoint * @param pos_sp_curr current position setpoint * @param pos_sp_next next position setpoint */ void 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, const position_setpoint_s &pos_sp_next); + const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next); /** @@ -703,11 +612,13 @@ private: /** * @brief Controls user commanded altitude, airspeed, and bearing. * + * @param now Current system time [us] * @param control_interval Time since last position control call [s] * @param curr_pos Current 2D local position vector of vehicle [m] * @param ground_speed Local 2D ground speed of vehicle [m/s] */ - void control_manual_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed); + void control_manual_position(const hrt_abstime now, const float control_interval, const Vector2d &curr_pos, + const Vector2f &ground_speed); /** * @brief Holds the initial heading during the course of a transition to hover. Used when there is no local @@ -724,26 +635,8 @@ private: void control_backtransition_line_follow(const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr); - float get_tecs_pitch(); - float get_tecs_thrust(); - float get_manual_airspeed_setpoint(); - /** - * @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 Minimum calibrated airspeed [m/s] - * @param ground_speed Vehicle ground velocity vector (NE) [m/s] - * @param in_takeoff_situation Vehicle is currently in a takeoff situation - * @return Adjusted calibrated airspeed setpoint [m/s] - */ - float adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint, - float calibrated_min_airspeed, const Vector2f &ground_speed, bool in_takeoff_situation = false); - void reset_takeoff_state(); void reset_landing_state(); @@ -758,39 +651,7 @@ private: void publishOrbitStatus(const position_setpoint_s pos_sp); - SlewRate _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 +699,6 @@ private: void initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev, const float land_point_alt, const Vector2f &local_position, const Vector2f &local_land_point); - /* - * Checks if the vehicle satisfies conditions for low-height flight - * - * @return bool True if conditions are satisfied, false otherwise - */ - bool checkLowHeightConditions(); - - /* - * Updates TECS altitude time constant according to the is_low_height parameter. - * - * @param is_low_height Boolean flag defining whether we are in low-height flight - * @param dt Update time step [s] - */ - void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt); - /* * Waypoint handling logic following closely to the ECL_L1_Pos_Controller * method of the same name. Takes two waypoints, steering the vehicle to track @@ -864,9 +710,10 @@ private: * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void navigateWaypoints(const matrix::Vector2f &start_waypoint, const matrix::Vector2f &end_waypoint, - const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel, - const matrix::Vector2f &wind_vel); + DirectionalGuidanceOutput navigateWaypoints(const matrix::Vector2f &start_waypoint, + const matrix::Vector2f &end_waypoint, + const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel, + const matrix::Vector2f &wind_vel); /* * Takes one waypoint and steers the vehicle towards this. @@ -879,8 +726,8 @@ private: * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos, - const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel); + DirectionalGuidanceOutput navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos, + const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel); /* * Line (infinite) following logic. Two points on the line are used to define the @@ -893,8 +740,9 @@ private: * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void 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); + DirectionalGuidanceOutput 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); /* * Line (infinite) following logic. One point on the line and a line bearing are used to define @@ -907,8 +755,9 @@ private: * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void navigateLine(const Vector2f &point_on_line, const float line_bearing, const Vector2f &vehicle_pos, - const Vector2f &ground_vel, const Vector2f &wind_vel); + DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line, const float line_bearing, + const Vector2f &vehicle_pos, + const Vector2f &ground_vel, const Vector2f &wind_vel); /* * Loitering (unlimited) logic. Takes loiter center, radius, and direction and @@ -922,9 +771,9 @@ private: * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos, - float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel, - const matrix::Vector2f &wind_vel); + DirectionalGuidanceOutput navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos, + float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel, + const matrix::Vector2f &wind_vel); /* * Path following logic. Takes poisiton, path tangent, curvature and @@ -939,9 +788,10 @@ private: * @param[in] wind_vel Wind velocity vector [m/s] * @param[in] curvature of the path setpoint [1/m] */ - void 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); + DirectionalGuidanceOutput 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); /* * Navigate on a fixed bearing. @@ -954,23 +804,22 @@ private: * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, const matrix::Vector2f &ground_vel, - const matrix::Vector2f &wind_vel); + DirectionalGuidanceOutput navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, + const matrix::Vector2f &ground_vel, + const matrix::Vector2f &wind_vel); + + void control_idle(); + void publish_lateral_guidance_status(const hrt_abstime now); + + float rollAngleToLateralAccel(float roll_body) const; DEFINE_PARAMETERS( - (ParamFloat) _param_fw_gnd_spd_min, - - (ParamFloat) _param_fw_pn_r_slew_max, (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 +829,46 @@ private: (ParamFloat) _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_ diff --git a/src/modules/fw_pos_control/figure_eight/FigureEight.cpp b/src/modules/fw_pos_control/figure_eight/FigureEight.cpp index d0ce50a033..9ab672d964 100644 --- a/src/modules/fw_pos_control/figure_eight/FigureEight.cpp +++ b/src/modules/fw_pos_control/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_pos_control/figure_eight/FigureEight.hpp index b3c55c840b..297e07942c 100644 --- a/src/modules/fw_pos_control/figure_eight/FigureEight.hpp +++ b/src/modules/fw_pos_control/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 @@ public: * * @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 @@ public: 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 @@ public: */ matrix::Vector2f getClosestPoint() const {return _closest_point_on_path;}; - private: /** * @brief @@ -172,12 +157,11 @@ private: * @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 @@ private: * @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 @@ private: * @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 @@ private: */ 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_pos_control/fw_path_navigation_params.c index 2e2005997a..9f82c22fb4 100644 --- a/src/modules/fw_pos_control/fw_path_navigation_params.c +++ b/src/modules/fw_pos_control/fw_path_navigation_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 * @@ -178,20 +122,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 * @@ -423,157 +353,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 * @@ -590,75 +369,6 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f); */ 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 * @@ -672,31 +382,6 @@ PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f); */ 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. * @@ -886,20 +571,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 * diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 544ecae871..691c57adb4 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,12 @@ 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); // multi topics add_optional_topic_multi("actuator_outputs", 100, 3); diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index ac96b8586f..5ecde816d1 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