mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
FW Position Controller rework
- split up old module into two, one handling setpoint generation, one control - add lateral and longitudinal control setpoints topics that can also be injected from companion computer - add configuration topics that (optionally) configure the controller with limits and momentary settings Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
Silvan Fuhrer
parent
52f0ef927d
commit
779a55c6dc
@ -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
|
||||
|
||||
#
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
10
msg/FixedWingLateralGuidanceStatus.msg
Normal file
10
msg/FixedWingLateralGuidanceStatus.msg
Normal file
@ -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)
|
||||
7
msg/FixedWingLateralSetpoint.msg
Normal file
7
msg/FixedWingLateralSetpoint.msg
Normal file
@ -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.
|
||||
4
msg/FixedWingLateralStatus.msg
Normal file
4
msg/FixedWingLateralStatus.msg
Normal file
@ -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]
|
||||
9
msg/FixedWingLongitudinalSetpoint.msg
Normal file
9
msg/FixedWingLongitudinalSetpoint.msg
Normal file
@ -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
|
||||
3
msg/LateralControlConfiguration.msg
Normal file
3
msg/LateralControlConfiguration.msg
Normal file
@ -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
|
||||
11
msg/LongitudinalControlConfiguration.msg
Normal file
11
msg/LongitudinalControlConfiguration.msg
Normal file
@ -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
|
||||
@ -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]
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
64
src/lib/npfg/AirspeedDirectionController.cpp
Normal file
64
src/lib/npfg/AirspeedDirectionController.cpp
Normal file
@ -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 <matrix/math.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
75
src/lib/npfg/AirspeedDirectionController.hpp
Normal file
75
src/lib/npfg/AirspeedDirectionController.hpp
Normal file
@ -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 <tstastny@ethz.ch>
|
||||
* Refactored to better suite new control API: Roman Bapst <roman@auterion.com>
|
||||
*
|
||||
* * 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
|
||||
@ -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)
|
||||
|
||||
107
src/lib/npfg/CourseToAirspeedRefMapper.cpp
Normal file
107
src/lib/npfg/CourseToAirspeedRefMapper.cpp
Normal file
@ -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;
|
||||
// }
|
||||
136
src/lib/npfg/CourseToAirspeedRefMapper.hpp
Normal file
136
src/lib/npfg/CourseToAirspeedRefMapper.hpp
Normal file
@ -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 <tstastny@ethz.ch>
|
||||
* Refactored to better suite new control API: Roman Bapst <roman@auterion.com>
|
||||
*
|
||||
* * 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 <matrix/math.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
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
|
||||
315
src/lib/npfg/DirectionalGuidance.cpp
Normal file
315
src/lib/npfg/DirectionalGuidance.cpp
Normal file
@ -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_);
|
||||
}
|
||||
@ -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 <tstastny@ethz.ch>
|
||||
* Original Author: Thomas Stastny <tstastny@ethz.ch>
|
||||
* Refactored to better suite new control API: Roman Bapst <roman@auterion.com>
|
||||
*
|
||||
* 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 <matrix/math.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
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
|
||||
@ -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 <lib/geo/geo.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <float.h>
|
||||
|
||||
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
|
||||
15
src/modules/fw_lateral_longitudinal_control/CMakeLists.txt
Normal file
15
src/modules/fw_lateral_longitudinal_control/CMakeLists.txt
Normal file
@ -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}
|
||||
)
|
||||
@ -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 <px4_platform_common/events.h>
|
||||
|
||||
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);
|
||||
}
|
||||
@ -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 <float.h>
|
||||
#include <fw_performance_model/PerformanceModel.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <lib/atmosphere/atmosphere.h>
|
||||
#include <lib/npfg/CourseToAirspeedRefMapper.hpp>
|
||||
#include <lib/npfg/AirspeedDirectionController.hpp>
|
||||
#include <lib/tecs/TECS.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
|
||||
#include <slew_rate/SlewRate.hpp>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/airspeed_validated.h>
|
||||
#include <uORB/topics/fixed_wing_lateral_setpoint.h>
|
||||
#include <uORB/topics/fixed_wing_lateral_status.h>
|
||||
#include <uORB/topics/fixed_wing_longitudinal_setpoint.h>
|
||||
#include <uORB/topics/flight_phase_estimation.h>
|
||||
#include <uORB/topics/lateral_control_configuration.h>
|
||||
#include <uORB/topics/longitudinal_control_configuration.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/tecs_status.h>
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/wind.h>
|
||||
|
||||
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<FwLateralLongitudinalControl>, 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<vehicle_control_mode_s> _control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::SubscriptionData<vehicle_air_data_s> _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_s> _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 <vehicle_attitude_setpoint_s> _attitude_sp_pub;
|
||||
uORB::Publication <tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
|
||||
uORB::PublicationData <flight_phase_estimation_s> _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)};
|
||||
uORB::Publication <fixed_wing_lateral_status_s> _fixed_wing_lateral_status_pub{ORB_ID(fixed_wing_lateral_status)};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
|
||||
(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
|
||||
(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad,
|
||||
(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,
|
||||
(ParamFloat<px4::params::FW_P_LIM_MAX>) _param_fw_p_lim_max,
|
||||
(ParamFloat<px4::params::FW_P_LIM_MIN>) _param_fw_p_lim_min,
|
||||
(ParamFloat<px4::params::FW_PN_R_SLEW_MAX>) _param_fw_pn_r_slew_max,
|
||||
(ParamFloat<px4::params::FW_T_HRATE_FF>) _param_fw_t_hrate_ff,
|
||||
(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
|
||||
(ParamFloat<px4::params::FW_T_F_ALT_ERR>) _param_fw_t_fast_alt_err,
|
||||
(ParamFloat<px4::params::FW_T_THR_INTEG>) _param_fw_t_thr_integ,
|
||||
(ParamFloat<px4::params::FW_T_I_GAIN_PIT>) _param_fw_t_I_gain_pit,
|
||||
(ParamFloat<px4::params::FW_T_PTCH_DAMP>) _param_fw_t_ptch_damp,
|
||||
(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
|
||||
(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,
|
||||
(ParamFloat<px4::params::FW_T_TAS_TC>) _param_fw_t_tas_error_tc,
|
||||
(ParamFloat<px4::params::FW_T_THR_DAMPING>) _param_fw_t_thr_damping,
|
||||
(ParamFloat<px4::params::FW_T_VERT_ACC>) _param_fw_t_vert_acc,
|
||||
(ParamFloat<px4::params::FW_T_STE_R_TC>) _param_ste_rate_time_const,
|
||||
(ParamFloat<px4::params::FW_T_SEB_R_FF>) _param_seb_rate_ff,
|
||||
(ParamFloat<px4::params::FW_T_SPD_STD>) _param_speed_standard_dev,
|
||||
(ParamFloat<px4::params::FW_T_SPD_DEV_STD>) _param_speed_rate_standard_dev,
|
||||
(ParamFloat<px4::params::FW_T_SPD_PRC_STD>) _param_process_noise_standard_dev,
|
||||
(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
|
||||
(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
|
||||
(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,
|
||||
(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
|
||||
(ParamFloat<px4::params::FW_THR_SLEW_MAX>) _param_fw_thr_slew_max,
|
||||
(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
|
||||
(ParamFloat<px4::params::FW_T_THR_LOW_HGT>) _param_fw_t_thr_low_hgt,
|
||||
(ParamFloat<px4::params::FW_WIND_ARSP_SC>) _param_fw_wind_arsp_sc,
|
||||
(ParamFloat<px4::params::FW_GND_SPD_MIN>) _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<float> _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<float> _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<float> _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
|
||||
5
src/modules/fw_lateral_longitudinal_control/Kconfig
Normal file
5
src/modules/fw_lateral_longitudinal_control/Kconfig
Normal file
@ -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
|
||||
284
src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.c
Normal file
284
src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.c
Normal file
@ -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);
|
||||
@ -60,6 +60,8 @@ px4_add_module(
|
||||
SRCS
|
||||
FixedwingPositionControl.cpp
|
||||
FixedwingPositionControl.hpp
|
||||
ControllerConfigurationHandler.cpp
|
||||
ControllerConfigurationHandler.hpp
|
||||
DEPENDS
|
||||
${POSCONTROL_DEPENDENCIES}
|
||||
)
|
||||
|
||||
139
src/modules/fw_pos_control/ControllerConfigurationHandler.cpp
Normal file
139
src/modules/fw_pos_control/ControllerConfigurationHandler.cpp
Normal file
@ -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 <drivers/drv_hrt.h>
|
||||
|
||||
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;
|
||||
}
|
||||
113
src/modules/fw_pos_control/ControllerConfigurationHandler.hpp
Normal file
113
src/modules/fw_pos_control/ControllerConfigurationHandler.hpp
Normal file
@ -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 <uORB/topics/longitudinal_control_configuration.h>
|
||||
#include <uORB/topics/lateral_control_configuration.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
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_control_configuration_s> _lateral_publisher{ORB_ID(lateral_control_configuration)};
|
||||
uORB::PublicationData<longitudinal_control_configuration_s> _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
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
x
Reference in New Issue
Block a user