fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add new fixed wing rate control module (fw_rate_control)

Browse Source 
* Fixedwing rate control into a separate module
* Start fw_rate_control for vtol
* Move over airspeed related parameters to fw_rate_control

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
JaeyoungLim 2023-01-04 17:14:00 +01:00 committed by GitHub
parent 4c2c06060d
commit 21e88f64b4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
59 changed files with 1736 additions and 1260 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d/rc.fw_apps
View File

@ -18,6 +18,7 @@ control_allocator start
#
# Start attitude controller.
#
fw_rate_control start
fw_att_control start
fw_pos_control_l1 start
airspeed_selector start

1
ROMFS/px4fmu_common/init.d/rc.vtol_apps
View File

@ -35,6 +35,7 @@ then
mc_autotune_attitude_control start
fi
fw_rate_control start vtol
fw_att_control start vtol
fw_pos_control_l1 start vtol
fw_autotune_attitude_control start vtol

1
boards/airmind/mindpx-v2/default.px4board
View File

@ -46,6 +46,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/ark/fmu-v6x/default.px4board
View File

@ -41,6 +41,7 @@ CONFIG_MODULES_EKF2=y
CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y

2
boards/atl/mantis-edu/default.px4board
View File

@ -14,8 +14,6 @@ CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y

1
boards/av/x-v1/default.px4board
View File

@ -43,6 +43,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/beaglebone/blue/default.px4board
View File

@ -30,6 +30,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/cuav/nora/default.px4board
View File

@ -50,6 +50,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/cuav/x7pro/default.px4board
View File

@ -51,6 +51,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/cubepilot/cubeorange/default.px4board
View File

@ -44,6 +44,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/cubepilot/cubeyellow/default.px4board
View File

@ -46,6 +46,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/emlid/navio2/default.px4board
View File

@ -32,6 +32,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/holybro/durandal-v1/default.px4board
View File

@ -44,6 +44,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/holybro/kakuteh7/default.px4board
View File

@ -37,6 +37,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/holybro/pix32v5/default.px4board
View File

@ -50,6 +50,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/matek/h743-mini/default.px4board
View File

@ -29,6 +29,7 @@ CONFIG_MODULES_EKF2=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y

1
boards/matek/h743-slim/default.px4board
View File

@ -30,6 +30,7 @@ CONFIG_MODULES_EKF2=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y

1
boards/matek/h743/default.px4board
View File

@ -31,6 +31,7 @@ CONFIG_MODULES_EKF2=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y

1
boards/modalai/fc-v1/default.px4board
View File

@ -48,6 +48,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/modalai/fc-v2/default.px4board
View File

@ -45,6 +45,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/mro/ctrl-zero-classic/default.px4board
View File

@ -43,6 +43,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/mro/ctrl-zero-f7-oem/default.px4board
View File

@ -44,6 +44,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/mro/ctrl-zero-f7/default.px4board
View File

@ -44,6 +44,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/mro/ctrl-zero-h7-oem/default.px4board
View File

@ -43,6 +43,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/mro/ctrl-zero-h7/default.px4board
View File

@ -44,6 +44,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/mro/pixracerpro/default.px4board
View File

@ -43,6 +43,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/mro/x21-777/default.px4board
View File

@ -45,6 +45,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/mro/x21/default.px4board
View File

@ -46,6 +46,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/nxp/fmuk66-e/default.px4board
View File

@ -46,6 +46,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/nxp/fmuk66-v3/default.px4board
View File

@ -48,6 +48,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/px4/fmu-v2/fixedwing.px4board
View File

@ -9,4 +9,5 @@ CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y

1
boards/px4/fmu-v3/default.px4board
View File

@ -49,6 +49,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/px4/fmu-v4/default.px4board
View File

@ -50,6 +50,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/px4/fmu-v4pro/default.px4board
View File

@ -47,6 +47,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/px4/fmu-v5/default.px4board
View File

@ -55,6 +55,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

2
boards/px4/fmu-v5/uavcanv0periph.px4board
View File

@ -19,7 +19,9 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
CONFIG_MODULES_CAMERA_FEEDBACK=n
CONFIG_MODULES_ESC_BATTERY=n
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
CONFIG_MODULES_GYRO_FFT=n
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
CONFIG_MODULES_MICRODDS_CLIENT=n
CONFIG_MODULES_ROVER_POS_CONTROL=n
CONFIG_MODULES_TEMPERATURE_COMPENSATION=n

1
boards/px4/fmu-v5x/default.px4board
View File

@ -55,6 +55,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/px4/fmu-v6c/default.px4board
View File

@ -40,6 +40,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/px4/fmu-v6u/default.px4board
View File

@ -47,6 +47,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/px4/fmu-v6x/default.px4board
View File

@ -49,6 +49,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/px4/raspberrypi/default.px4board
View File

@ -29,6 +29,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/px4/sitl/default.px4board
View File

@ -16,6 +16,7 @@ CONFIG_MODULES_EKF2=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/scumaker/pilotpi/default.px4board
View File

@ -29,6 +29,7 @@ CONFIG_MODULES_EKF2=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

1
boards/sky-drones/smartap-airlink/default.px4board
View File

@ -47,6 +47,7 @@ CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y

2
src/modules/fw_att_control/CMakeLists.txt
View File

@ -45,6 +45,4 @@ px4_add_module(
ecl_yaw_controller.cpp
DEPENDS
px4_work_queue
RateControl
SlewRate
)

534
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -37,29 +37,16 @@ using namespace time_literals;
using namespace matrix;
using math::constrain;
using math::interpolate;
using math::radians;
FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
_actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)),
_actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)),
_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
/* fetch initial parameter values */
parameters_update();
// set initial maximum body rate setpoints
_roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get()));
_pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get()));
_pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get()));
_yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get()));
_rate_ctrl_status_pub.advertise();
_spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate);
_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate);
}
FixedwingAttitudeControl::~FixedwingAttitudeControl()
@ -78,116 +65,55 @@ FixedwingAttitudeControl::init()
return true;
}
int
void
FixedwingAttitudeControl::parameters_update()
{
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_param_fw_p_tc.get());
/* roll control parameters */
_roll_ctrl.set_time_constant(_param_fw_r_tc.get());
_roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get()));
_pitch_ctrl.set_time_constant(_param_fw_p_tc.get());
_pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get()));
_pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get()));
_yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get()));
/* wheel control parameters */
_wheel_ctrl.set_k_p(_param_fw_wr_p.get());
_wheel_ctrl.set_k_i(_param_fw_wr_i.get());
_wheel_ctrl.set_k_ff(_param_fw_wr_ff.get());
_wheel_ctrl.set_integrator_max(_param_fw_wr_imax.get());
_wheel_ctrl.set_max_rate(radians(_param_fw_w_rmax.get()));
const Vector3f rate_p = Vector3f(_param_fw_rr_p.get(), _param_fw_pr_p.get(), _param_fw_yr_p.get());
const Vector3f rate_i = Vector3f(_param_fw_rr_i.get(), _param_fw_pr_i.get(), _param_fw_yr_i.get());
const Vector3f rate_d = Vector3f(_param_fw_rr_d.get(), _param_fw_pr_d.get(), _param_fw_yr_d.get());
_rate_control.setGains(rate_p, rate_i, rate_d);
_rate_control.setIntegratorLimit(
Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get()));
_rate_control.setFeedForwardGain(
// set FF gains to 0 as we add the FF control outside of the rate controller
Vector3f(0.f, 0.f, 0.f));
return PX4_OK;
}
void
FixedwingAttitudeControl::vehicle_control_mode_poll()
{
_vcontrol_mode_sub.update(&_vcontrol_mode);
if (_vehicle_status.is_vtol) {
const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& !_vehicle_status.in_transition_mode;
const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _vehicle_status.is_vtol_tailsitter;
if (is_hovering || is_tailsitter_transition) {
_vcontrol_mode.flag_control_attitude_enabled = false;
_vcontrol_mode.flag_control_manual_enabled = false;
}
}
}
void
FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
{
const bool is_tailsitter_transition = _vehicle_status.is_vtol_tailsitter && _vehicle_status.in_transition_mode;
const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) {
if (_vcontrol_mode.flag_control_manual_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
// Always copy the new manual setpoint, even if it wasn't updated, to fill the actuators with valid values
if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) {
if (!_vcontrol_mode.flag_control_climb_rate_enabled) {
if (!_vcontrol_mode.flag_control_climb_rate_enabled & _vcontrol_mode.flag_control_attitude_enabled) {
const float throttle = (_manual_control_setpoint.throttle + 1.f) * .5f;
// STABILIZED mode generate the attitude setpoint from manual user inputs
if (_vcontrol_mode.flag_control_attitude_enabled) {
// STABILIZED mode generate the attitude setpoint from manual user inputs
_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get());
_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get());
_att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get())
+ radians(_param_fw_psp_off.get());
_att_sp.pitch_body = constrain(_att_sp.pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
_att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get())
+ radians(_param_fw_psp_off.get());
_att_sp.pitch_body = constrain(_att_sp.pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
_att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;
_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
_att_sp.thrust_body[0] = throttle;
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
_att_sp.reset_integral = false;
_att_sp.reset_integral = false;
_att_sp.timestamp = hrt_absolute_time();
_att_sp.timestamp = hrt_absolute_time();
_attitude_sp_pub.publish(_att_sp);
} else if (_vcontrol_mode.flag_control_rates_enabled &&
!_vcontrol_mode.flag_control_attitude_enabled) {
// RATE mode we need to generate the rate setpoint from manual user inputs
_rates_sp.timestamp = hrt_absolute_time();
_rates_sp.roll = _manual_control_setpoint.roll * radians(_param_fw_acro_x_max.get());
_rates_sp.pitch = -_manual_control_setpoint.pitch * radians(_param_fw_acro_y_max.get());
_rates_sp.yaw = _manual_control_setpoint.yaw * radians(_param_fw_acro_z_max.get());
_rates_sp.thrust_body[0] = throttle;
_rate_sp_pub.publish(_rates_sp);
} else {
/* manual/direct control */
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get();
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
-_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
_actuator_controls.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = throttle;
_landing_gear_wheel.normalized_wheel_setpoint = _manual_control_setpoint.yaw;
}
_attitude_sp_pub.publish(_att_sp);
}
}
}
@ -215,7 +141,7 @@ FixedwingAttitudeControl::vehicle_land_detected_poll()
}
}
float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
float FixedwingAttitudeControl::get_airspeed_constrained()
{
_airspeed_validated_sub.update();
const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().calibrated_airspeed_m_s)
@ -238,19 +164,7 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
}
}
/*
* For scaling our actuators using anything less than the stall
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and it's the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_stall.get(),
_param_fw_airspd_max.get()), 0.1f, 1000.0f);
_airspeed_scaling = (_param_fw_arsp_scale_en.get()) ? (_param_fw_airspd_trim.get() / airspeed_constrained) : 1.0f;
return airspeed;
return math::constrain(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_max.get());
}
void FixedwingAttitudeControl::Run()
@ -267,7 +181,7 @@ void FixedwingAttitudeControl::Run()
if (_att_sub.updated() || (hrt_elapsed_time(&_last_run) > 20_ms)) {
// only update parameters if they changed
bool params_updated = _parameter_update_sub.updated();
const bool params_updated = _parameter_update_sub.updated();
// check for parameter updates
if (params_updated) {
@ -303,11 +217,7 @@ void FixedwingAttitudeControl::Run()
vehicle_angular_velocity_s angular_velocity{};
_vehicle_rates_sub.copy(&angular_velocity);
float rollspeed = angular_velocity.xyz[0];
float pitchspeed = angular_velocity.xyz[1];
float yawspeed = angular_velocity.xyz[2];
const Vector3f rates(rollspeed, pitchspeed, yawspeed);
const Vector3f angular_accel{angular_velocity.xyz_derivative};
float yawspeed = angular_velocity.xyz[2]; // only used for wheel controller
if (_vehicle_status.is_vtol_tailsitter) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
@ -347,9 +257,7 @@ void FixedwingAttitudeControl::Run()
_R = R_adapted;
/* lastly, roll- and yawspeed have to be swaped */
float helper = rollspeed;
rollspeed = -yawspeed;
yawspeed = helper;
yawspeed = angular_velocity.xyz[0];
}
const matrix::Eulerf euler_angles(_R);
@ -358,10 +266,15 @@ void FixedwingAttitudeControl::Run()
vehicle_attitude_setpoint_poll();
// vehicle status update must be before the vehicle_control_mode_poll(), otherwise rate sp are not published during whole transition
// vehicle status update must be before the vehicle_control_mode poll, otherwise rate sp are not published during whole transition
_vehicle_status_sub.update(&_vehicle_status);
const bool is_in_transition_except_tailsitter = _vehicle_status.in_transition_mode
&& !_vehicle_status.is_vtol_tailsitter;
const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
_in_fw_or_transition_wo_tailsitter_transition = is_fixed_wing || is_in_transition_except_tailsitter;
_vehicle_control_mode_sub.update(&_vcontrol_mode);
vehicle_control_mode_poll();
vehicle_land_detected_poll();
// the position controller will not emit attitude setpoints in some modes
@ -376,26 +289,18 @@ void FixedwingAttitudeControl::Run()
/* if we are in rotary wing mode, do nothing */
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) {
_spoiler_setpoint_with_slewrate.setForcedValue(0.f);
_flaps_setpoint_with_slewrate.setForcedValue(0.f);
perf_end(_loop_perf);
return;
}
controlFlaps(dt);
controlSpoilers(dt);
if (_vcontrol_mode.flag_control_rates_enabled) {
const float airspeed = get_airspeed_and_update_scaling();
/* Reset integrators if commanded by attitude setpoint, or the aircraft is on ground
* or a multicopter (but not transitioning VTOL or tailsitter)
*/
if (_att_sp.reset_integral
|| _landed
|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) {
|| !_in_fw_or_transition_wo_tailsitter_transition) {
_rates_sp.reset_integral = true;
_wheel_ctrl.reset_integrator();
@ -404,26 +309,25 @@ void FixedwingAttitudeControl::Run()
_rates_sp.reset_integral = false;
}
// update saturation status from control allocation feedback
control_allocator_status_s control_allocator_status;
float groundspeed_scale = 1.f;
if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) {
Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative;
if (wheel_control) {
if (_local_pos_sub.updated()) {
vehicle_local_position_s vehicle_local_position;
if (!control_allocator_status.torque_setpoint_achieved) {
for (size_t i = 0; i < 3; i++) {
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
saturation_positive(i) = true;
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true;
}
if (_local_pos_sub.copy(&vehicle_local_position)) {
_groundspeed = sqrtf(vehicle_local_position.vx * vehicle_local_position.vx + vehicle_local_position.vy *
vehicle_local_position.vy);
}
}
// TODO: send the unallocated value directly for better anti-windup
_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
// Use stall airspeed to calculate ground speed scaling region. Don't scale below gspd_scaling_trim
float gspd_scaling_trim = (_param_fw_airspd_stall.get());
if (_groundspeed > gspd_scaling_trim) {
groundspeed_scale = gspd_scaling_trim / _groundspeed;
}
}
/* Prepare data for attitude controllers */
@ -431,91 +335,19 @@ void FixedwingAttitudeControl::Run()
control_input.roll = euler_angles.phi();
control_input.pitch = euler_angles.theta();
control_input.yaw = euler_angles.psi();
control_input.body_x_rate = rollspeed;
control_input.body_y_rate = pitchspeed;
control_input.body_z_rate = yawspeed;
control_input.roll_setpoint = _att_sp.roll_body;
control_input.pitch_setpoint = _att_sp.pitch_body;
control_input.yaw_setpoint = _att_sp.yaw_body;
control_input.euler_roll_rate_setpoint = _roll_ctrl.get_euler_rate_setpoint();
control_input.euler_pitch_rate_setpoint = _pitch_ctrl.get_euler_rate_setpoint();
control_input.euler_yaw_rate_setpoint = _yaw_ctrl.get_euler_rate_setpoint();
control_input.airspeed_min = _param_fw_airspd_stall.get();
control_input.airspeed_max = _param_fw_airspd_max.get();
control_input.airspeed = airspeed;
if (wheel_control) {
_local_pos_sub.update(&_local_pos);
/* Use stall airspeed to calculate ground speed scaling region.
* Don't scale below gspd_scaling_trim
*/
float groundspeed = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
float gspd_scaling_trim = (_param_fw_airspd_stall.get());
control_input.groundspeed = groundspeed;
if (groundspeed > gspd_scaling_trim) {
control_input.groundspeed_scaler = gspd_scaling_trim / groundspeed;
} else {
control_input.groundspeed_scaler = 1.0f;
}
}
/* reset body angular rate limits on mode change */
if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) {
if (_vcontrol_mode.flag_control_attitude_enabled
|| _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get()));
_pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get()));
_pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get()));
_yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get()));
} else {
_roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get()));
_pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get()));
_pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get()));
_yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get()));
}
}
_flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled;
/* bi-linear interpolation over airspeed for actuator trim scheduling */
float trim_roll = _param_trim_roll.get();
float trim_pitch = _param_trim_pitch.get();
float trim_yaw = _param_trim_yaw.get();
if (airspeed < _param_fw_airspd_trim.get()) {
trim_roll += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
_param_fw_dtrim_r_vmin.get(),
0.0f);
trim_pitch += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
_param_fw_dtrim_p_vmin.get(),
0.0f);
trim_yaw += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
_param_fw_dtrim_y_vmin.get(),
0.0f);
} else {
trim_roll += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_param_fw_dtrim_r_vmax.get());
trim_pitch += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_param_fw_dtrim_p_vmax.get());
trim_yaw += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_param_fw_dtrim_y_vmax.get());
}
/* add trim increment if flaps are deployed */
trim_roll += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_r_flps.get();
trim_pitch += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_flps.get();
// add trim increment from spoilers (only pitch)
trim_pitch += _spoiler_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_spoil.get();
control_input.airspeed_constrained = get_airspeed_constrained();
control_input.groundspeed = _groundspeed;
control_input.groundspeed_scaler = groundspeed_scale;
/* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled) {
if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
_roll_ctrl.control_attitude(dt, control_input);
_pitch_ctrl.control_attitude(dt, control_input);
@ -532,7 +364,6 @@ void FixedwingAttitudeControl::Run()
Vector3f body_rates_setpoint = Vector3f(_roll_ctrl.get_body_rate_setpoint(), _pitch_ctrl.get_body_rate_setpoint(),
_yaw_ctrl.get_body_rate_setpoint());
const hrt_abstime now = hrt_absolute_time();
autotune_attitude_control_status_s pid_autotune;
matrix::Vector3f bodyrate_autotune_ff;
@ -541,7 +372,7 @@ void FixedwingAttitudeControl::Run()
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST)
&& ((now - pid_autotune.timestamp) < 1_s)) {
&& ((hrt_absolute_time() - pid_autotune.timestamp) < 1_s)) {
bodyrate_autotune_ff = matrix::Vector3f(pid_autotune.rate_sp);
body_rates_setpoint += bodyrate_autotune_ff;
@ -550,7 +381,7 @@ void FixedwingAttitudeControl::Run()
/* add yaw rate setpoint from sticks in all attitude-controlled modes */
if (_vcontrol_mode.flag_control_manual_enabled) {
body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.yaw * radians(_param_fw_man_yr_max.get()),
body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.yaw * radians(_param_fw_y_rmax.get()),
-radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get()));
}
@ -565,124 +396,29 @@ void FixedwingAttitudeControl::Run()
}
}
if (_vcontrol_mode.flag_control_rates_enabled) {
_rates_sp_sub.update(&_rates_sp);
// wheel control
float wheel_u = 0.f;
const Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw);
if (_vcontrol_mode.flag_control_manual_enabled) {
// always direct control of steering wheel with yaw stick in manual modes
wheel_u = _manual_control_setpoint.yaw;
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt,
_landed);
float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0);
float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
(PX4_ISFINITE(roll_u)) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll;
float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1);
float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
(PX4_ISFINITE(pitch_u)) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch;
const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2);
const float yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward;
// wheel control
float wheel_u = 0.f;
if (_vcontrol_mode.flag_control_manual_enabled) {
// always direct control of steering wheel with yaw stick in manual modes
wheel_u = _manual_control_setpoint.yaw;
} else {
// XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from
// position controller during auto modes _manual_control_setpoint.yaw gets passed
// whenever nudging is enabled, otherwise zero
wheel_u = wheel_control ? _wheel_ctrl.control_bodyrate(dt, control_input)
+ _att_sp.yaw_sp_move_rate : 0.f;
}
_landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(wheel_u) ? wheel_u : 0.f;
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw,
-1.f, 1.f) : trim_yaw;
if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u) || _rates_sp.reset_integral) {
_rate_control.resetIntegral();
}
if (!PX4_ISFINITE(wheel_u)) {
_wheel_ctrl.reset_integrator();
}
/* throttle passed through if it is finite */
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] =
(PX4_ISFINITE(_rates_sp.thrust_body[0])) ? _rates_sp.thrust_body[0] : 0.0f;
/* scale effort by battery status */
if (_param_fw_bat_scale_en.get() &&
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
if (_battery_status_sub.updated()) {
battery_status_s battery_status{};
if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
_battery_scale = battery_status.scale;
}
}
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
}
} else {
// XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from
// position controller during auto modes _manual_control_setpoint.r gets passed
// whenever nudging is enabled, otherwise zero
wheel_u = wheel_control ? _wheel_ctrl.control_bodyrate(dt, control_input)
+ _att_sp.yaw_sp_move_rate : 0.f;
}
// publish rate controller status
rate_ctrl_status_s rate_ctrl_status{};
_rate_control.getRateControlStatus(rate_ctrl_status);
rate_ctrl_status.timestamp = hrt_absolute_time();
if (wheel_control) {
rate_ctrl_status.wheel_rate_integ = _wheel_ctrl.get_integrator();
}
_rate_ctrl_status_pub.publish(rate_ctrl_status);
_landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(wheel_u) ? wheel_u : 0.f;
_landing_gear_wheel.timestamp = hrt_absolute_time();
_landing_gear_wheel_pub.publish(_landing_gear_wheel);
} else {
// full manual
_rate_control.resetIntegral();
_wheel_ctrl.reset_integrator();
}
// Add feed-forward from roll control output to yaw control output
// This can be used to counteract the adverse yaw effect when rolling the plane
_actuator_controls.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get()
* constrain(_actuator_controls.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_actuator_controls.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
_actuator_controls.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
_actuator_controls.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
_actuator_controls.control[actuator_controls_s::INDEX_LANDING_GEAR] = _manual_control_setpoint.aux3;
/* lazily publish the setpoint only once available */
_actuator_controls.timestamp = hrt_absolute_time();
_actuator_controls.timestamp_sample = att.timestamp;
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled) {
_actuator_controls_0_pub.publish(_actuator_controls);
if (!_vehicle_status.is_vtol) {
publishTorqueSetpoint(angular_velocity.timestamp_sample);
publishThrustSetpoint(angular_velocity.timestamp_sample);
}
}
_landing_gear_wheel.timestamp = hrt_absolute_time();
_landing_gear_wheel_pub.publish(_landing_gear_wheel);
updateActuatorControlsStatus(dt);
}
// backup schedule
@ -691,132 +427,6 @@ void FixedwingAttitudeControl::Run()
perf_end(_loop_perf);
}
void FixedwingAttitudeControl::publishTorqueSetpoint(const hrt_abstime &timestamp_sample)
{
vehicle_torque_setpoint_s v_torque_sp = {};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.timestamp_sample = timestamp_sample;
v_torque_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_ROLL];
v_torque_sp.xyz[1] = _actuator_controls.control[actuator_controls_s::INDEX_PITCH];
v_torque_sp.xyz[2] = _actuator_controls.control[actuator_controls_s::INDEX_YAW];
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
void FixedwingAttitudeControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
{
vehicle_thrust_setpoint_s v_thrust_sp = {};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.timestamp_sample = timestamp_sample;
v_thrust_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
void FixedwingAttitudeControl::controlFlaps(const float dt)
{
/* default flaps to center */
float flap_control = 0.0f;
/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled) {
flap_control = _manual_control_setpoint.flaps;
} else if (_vcontrol_mode.flag_control_auto_enabled) {
switch (_att_sp.apply_flaps) {
case vehicle_attitude_setpoint_s::FLAPS_OFF:
flap_control = 0.0f; // no flaps
break;
case vehicle_attitude_setpoint_s::FLAPS_LAND:
flap_control = _param_fw_flaps_lnd_scl.get();
break;
case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF:
flap_control = _param_fw_flaps_to_scl.get();
break;
}
}
// move the actual control value continuous with time, full flap travel in 1sec
_flaps_setpoint_with_slewrate.update(math::constrain(flap_control, 0.f, 1.f), dt);
}
void FixedwingAttitudeControl::controlSpoilers(const float dt)
{
float spoiler_control = 0.f;
if (_vcontrol_mode.flag_control_manual_enabled) {
switch (_param_fw_spoilers_man.get()) {
case 0:
break;
case 1:
spoiler_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? _manual_control_setpoint.flaps : 0.f;
break;
case 2:
spoiler_control = PX4_ISFINITE(_manual_control_setpoint.aux1) ? _manual_control_setpoint.aux1 : 0.f;
break;
}
} else if (_vcontrol_mode.flag_control_auto_enabled) {
switch (_att_sp.apply_spoilers) {
case vehicle_attitude_setpoint_s::SPOILERS_OFF:
spoiler_control = 0.f;
break;
case vehicle_attitude_setpoint_s::SPOILERS_LAND:
spoiler_control = _param_fw_spoilers_lnd.get();
break;
case vehicle_attitude_setpoint_s::SPOILERS_DESCEND:
spoiler_control = _param_fw_spoilers_desc.get();
break;
}
}
_spoiler_setpoint_with_slewrate.update(math::constrain(spoiler_control, 0.f, 1.f), dt);
}
void FixedwingAttitudeControl::updateActuatorControlsStatus(float dt)
{
for (int i = 0; i < 4; i++) {
float control_signal;
if (i <= actuator_controls_status_s::INDEX_YAW) {
// We assume that the attitude is actuated by control surfaces
// consuming power only when they move
control_signal = _actuator_controls.control[i] - _control_prev[i];
_control_prev[i] = _actuator_controls.control[i];
} else {
control_signal = _actuator_controls.control[i];
}
_control_energy[i] += control_signal * control_signal * dt;
}
_energy_integration_time += dt;
if (_energy_integration_time > 500e-3f) {
actuator_controls_status_s status;
status.timestamp = _actuator_controls.timestamp;
for (int i = 0; i < 4; i++) {
status.control_power[i] = _control_energy[i] / _energy_integration_time;
_control_energy[i] = 0.f;
}
_actuator_controls_status_pub.publish(status);
_energy_integration_time = 0.f;
}
}
int FixedwingAttitudeControl::task_spawn(int argc, char *argv[])
{
bool vtol = false;

139
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -33,18 +33,14 @@
#pragma once
#include <lib/rate_control/rate_control.hpp>
#include <drivers/drv_hrt.h>
#include "ecl_pitch_controller.h"
#include "ecl_roll_controller.h"
#include "ecl_wheel_controller.h"
#include "ecl_yaw_controller.h"
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <lib/slew_rate/SlewRate.hpp>
#include <matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
@ -54,20 +50,13 @@
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_status.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/autotune_attitude_control_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_allocator_status.h>
#include <uORB/topics/landing_gear_wheel.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@ -76,8 +65,6 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
using matrix::Eulerf;
using matrix::Quatf;
@ -86,9 +73,6 @@ using uORB::SubscriptionData;
using namespace time_literals;
static constexpr float kFlapSlewRate = 1.f; //minimum time from none to full flap deflection [s]
static constexpr float kSpoilerSlewRate = 1.f; //minimum time from none to full spoiler deflection [s]
class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeControl>, public ModuleParams,
public px4::ScheduledWorkItem
{
@ -110,42 +94,28 @@ public:
private:
void Run() override;
void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
uORB::SubscriptionCallbackWorkItem _att_sub{this, ORB_ID(vehicle_attitude)}; /**< vehicle attitude */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */
uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
uORB::SubscriptionMultiArray<control_allocator_status_s, 2> _control_allocator_status_subs{ORB_ID::control_allocator_status};
uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
uORB::Publication<actuator_controls_s> _actuator_controls_0_pub;
uORB::Publication<actuator_controls_status_s> _actuator_controls_status_pub;
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
uORB::Publication<landing_gear_wheel_s> _landing_gear_wheel_pub{ORB_ID(landing_gear_wheel)};
actuator_controls_s _actuator_controls{};
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_attitude_setpoint_s _att_sp{};
vehicle_control_mode_s _vcontrol_mode{};
vehicle_local_position_s _local_pos{};
vehicle_rates_setpoint_s _rates_sp{};
vehicle_status_s _vehicle_status{};
landing_gear_wheel_s _landing_gear_wheel{};
@ -156,76 +126,27 @@ private:
hrt_abstime _last_run{0};
float _airspeed_scaling{1.0f};
bool _landed{true};
float _battery_scale{1.0f};
bool _flag_control_attitude_enabled_last{false};
float _energy_integration_time{0.0f};
float _control_energy[4] {};
float _control_prev[3] {};
SlewRate<float> _spoiler_setpoint_with_slewrate;
SlewRate<float> _flaps_setpoint_with_slewrate;
float _groundspeed{0.f};
bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states
DEFINE_PARAMETERS(
(ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max,
(ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max,
(ParamFloat<px4::params::FW_ACRO_Z_MAX>) _param_fw_acro_z_max,
(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
(ParamInt<px4::params::FW_ARSP_SCALE_EN>) _param_fw_arsp_scale_en,
(ParamBool<px4::params::FW_BAT_SCALE_EN>) _param_fw_bat_scale_en,
(ParamFloat<px4::params::FW_DTRIM_P_FLPS>) _param_fw_dtrim_p_flps,
(ParamFloat<px4::params::FW_DTRIM_P_SPOIL>) _param_fw_dtrim_p_spoil,
(ParamFloat<px4::params::FW_DTRIM_P_VMAX>) _param_fw_dtrim_p_vmax,
(ParamFloat<px4::params::FW_DTRIM_P_VMIN>) _param_fw_dtrim_p_vmin,
(ParamFloat<px4::params::FW_DTRIM_R_FLPS>) _param_fw_dtrim_r_flps,
(ParamFloat<px4::params::FW_DTRIM_R_VMAX>) _param_fw_dtrim_r_vmax,
(ParamFloat<px4::params::FW_DTRIM_R_VMIN>) _param_fw_dtrim_r_vmin,
(ParamFloat<px4::params::FW_DTRIM_Y_VMAX>) _param_fw_dtrim_y_vmax,
(ParamFloat<px4::params::FW_DTRIM_Y_VMIN>) _param_fw_dtrim_y_vmin,
(ParamFloat<px4::params::FW_FLAPS_LND_SCL>) _param_fw_flaps_lnd_scl,
(ParamFloat<px4::params::FW_FLAPS_TO_SCL>) _param_fw_flaps_to_scl,
(ParamFloat<px4::params::FW_SPOILERS_LND>) _param_fw_spoilers_lnd,
(ParamFloat<px4::params::FW_SPOILERS_DESC>) _param_fw_spoilers_desc,
(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man,
(ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
(ParamFloat<px4::params::FW_MAN_P_SC>) _param_fw_man_p_sc,
(ParamFloat<px4::params::FW_MAN_R_MAX>) _param_fw_man_r_max,
(ParamFloat<px4::params::FW_MAN_R_SC>) _param_fw_man_r_sc,
(ParamFloat<px4::params::FW_MAN_Y_SC>) _param_fw_man_y_sc,
(ParamFloat<px4::params::FW_P_RMAX_NEG>) _param_fw_p_rmax_neg,
(ParamFloat<px4::params::FW_P_RMAX_POS>) _param_fw_p_rmax_pos,
(ParamFloat<px4::params::FW_P_TC>) _param_fw_p_tc,
(ParamFloat<px4::params::FW_PR_FF>) _param_fw_pr_ff,
(ParamFloat<px4::params::FW_PR_I>) _param_fw_pr_i,
(ParamFloat<px4::params::FW_PR_IMAX>) _param_fw_pr_imax,
(ParamFloat<px4::params::FW_PR_P>) _param_fw_pr_p,
(ParamFloat<px4::params::FW_PR_D>) _param_fw_pr_d,
(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
(ParamFloat<px4::params::FW_R_RMAX>) _param_fw_r_rmax,
(ParamFloat<px4::params::FW_R_TC>) _param_fw_r_tc,
(ParamFloat<px4::params::FW_RLL_TO_YAW_FF>) _param_fw_rll_to_yaw_ff,
(ParamFloat<px4::params::FW_RR_FF>) _param_fw_rr_ff,
(ParamFloat<px4::params::FW_RR_I>) _param_fw_rr_i,
(ParamFloat<px4::params::FW_RR_IMAX>) _param_fw_rr_imax,
(ParamFloat<px4::params::FW_RR_P>) _param_fw_rr_p,
(ParamFloat<px4::params::FW_RR_D>) _param_fw_rr_d,
(ParamBool<px4::params::FW_W_EN>) _param_fw_w_en,
(ParamFloat<px4::params::FW_W_RMAX>) _param_fw_w_rmax,
@ -234,51 +155,17 @@ private:
(ParamFloat<px4::params::FW_WR_IMAX>) _param_fw_wr_imax,
(ParamFloat<px4::params::FW_WR_P>) _param_fw_wr_p,
(ParamFloat<px4::params::FW_Y_RMAX>) _param_fw_y_rmax,
(ParamFloat<px4::params::FW_YR_FF>) _param_fw_yr_ff,
(ParamFloat<px4::params::FW_YR_I>) _param_fw_yr_i,
(ParamFloat<px4::params::FW_YR_IMAX>) _param_fw_yr_imax,
(ParamFloat<px4::params::FW_YR_P>) _param_fw_yr_p,
(ParamFloat<px4::params::FW_YR_D>) _param_fw_yr_d,
(ParamFloat<px4::params::TRIM_PITCH>) _param_trim_pitch,
(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw,
(ParamFloat<px4::params::FW_MAN_YR_MAX>) _param_fw_man_yr_max
(ParamFloat<px4::params::FW_Y_RMAX>) _param_fw_y_rmax
)
ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl;
ECL_YawController _yaw_ctrl;
ECL_WheelController _wheel_ctrl;
RateControl _rate_control; ///< class for rate control calculations
ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl;
ECL_YawController _yaw_ctrl;
ECL_WheelController _wheel_ctrl;
/**
* @brief Update flap control setting
*
* @param dt Current time delta [s]
*/
void controlFlaps(const float dt);
/**
* @brief Update spoiler control setting
*
* @param dt Current time delta [s]
*/
void controlSpoilers(const float dt);
void updateActuatorControlsStatus(float dt);
/**
* Update our local parameter cache.
*/
int parameters_update();
void vehicle_control_mode_poll();
void vehicle_manual_poll(const float yaw_body);
void vehicle_attitude_setpoint_poll();
void vehicle_land_detected_poll();
float get_airspeed_and_update_scaling();
void parameters_update();
void vehicle_manual_poll(const float yaw_body);
void vehicle_attitude_setpoint_poll();
void vehicle_land_detected_poll();
float get_airspeed_constrained();
};

15
src/modules/fw_att_control/ecl_controller.cpp
View File

@ -117,18 +117,3 @@ float ECL_Controller::get_integrator()
{
return _integrator;
}
float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed)
{
float airspeed_result = airspeed;
if (!PX4_ISFINITE(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed_result = 0.5f * (minspeed + maxspeed);
} else if (airspeed < minspeed) {
airspeed_result = minspeed;
}
return airspeed_result;
}

8
src/modules/fw_att_control/ecl_controller.h
View File

@ -55,18 +55,13 @@ struct ECL_ControlData {
float roll;
float pitch;
float yaw;
float body_x_rate;
float body_y_rate;
float body_z_rate;
float roll_setpoint;
float pitch_setpoint;
float yaw_setpoint;
float euler_roll_rate_setpoint;
float euler_pitch_rate_setpoint;
float euler_yaw_rate_setpoint;
float airspeed_min;
float airspeed_max;
float airspeed;
float airspeed_constrained;
float groundspeed;
float groundspeed_scaler;
};
@ -113,5 +108,4 @@ protected:
float _integrator;
float _euler_rate_setpoint;
float _body_rate_setpoint;
float constrain_airspeed(float airspeed, float minspeed, float maxspeed);
};

3
src/modules/fw_att_control/ecl_pitch_controller.cpp
View File

@ -49,8 +49,7 @@ float ECL_PitchController::control_attitude(const float dt, const ECL_ControlDat
if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed))) {
PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint))) {
return _body_rate_setpoint;
}

6
src/modules/fw_att_control/ecl_yaw_controller.cpp
View File

@ -48,7 +48,8 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.euler_pitch_rate_setpoint))) {
PX4_ISFINITE(ctl_data.euler_pitch_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_constrained))) {
return _body_rate_setpoint;
}
@ -81,8 +82,7 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
if (!inverted) {
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_euler_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed <
ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
_euler_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / ctl_data.airspeed_constrained;
/* Transform setpoint to body angular rates (jacobian) */
const float yaw_body_rate_setpoint_raw = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint +

567
src/modules/fw_att_control/fw_att_control_params.c
View File

@ -81,49 +81,6 @@ PARAM_DEFINE_FLOAT(FW_R_TC, 0.4f);
*/
PARAM_DEFINE_FLOAT(FW_P_TC, 0.4f);
/**
* Pitch rate proportional gain.
*
* Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @unit %/rad/s
* @min 0.0
* @max 2.0
* @decimal 3
* @increment 0.005
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f);
/**
* Pitch rate derivative gain.
*
* Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @unit %/rad/s
* @min 0.0
* @max 1.0
* @decimal 3
* @increment 0.005
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_D, 0.f);
/**
* Pitch rate integrator gain.
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @unit %/rad
* @min 0.0
* @max 1
* @decimal 3
* @increment 0.005
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_I, 0.1f);
/**
* Maximum positive / up pitch rate.
*
@ -154,76 +111,6 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
*/
PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
/**
* Pitch rate integrator limit
*
* The portion of the integrator part in the control surface deflection is
* limited to this value
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.05
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f);
/**
* Roll rate proportional Gain
*
* Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @unit %/rad/s
* @min 0.0
* @max 2.0
* @decimal 3
* @increment 0.005
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
/**
* Roll rate derivative Gain
*
* Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @unit %/rad/s
* @min 0.0
* @max 1.0
* @decimal 3
* @increment 0.005
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_D, 0.00f);
/**
* Roll rate integrator Gain
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @unit %/rad
* @min 0.0
* @max 1
* @decimal 3
* @increment 0.005
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_I, 0.1f);
/**
* Roll integrator anti-windup
*
* The portion of the integrator part in the control surface deflection is limited to this value.
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.05
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.4f);
/**
* Maximum roll rate
*
@ -239,63 +126,6 @@ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.4f);
*/
PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f);
/**
* Yaw rate proportional gain
*
* Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @unit %/rad/s
* @min 0.0
* @max 2.0
* @decimal 3
* @increment 0.005
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
/**
* Yaw rate derivative gain
*
* Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @unit %/rad/s
* @min 0.0
* @max 1.0
* @decimal 3
* @increment 0.005
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_D, 0.0f);
/**
* Yaw rate integrator gain
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @unit %/rad
* @min 0.0
* @max 1
* @decimal 1
* @increment 0.5
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_I, 0.1f);
/**
* Yaw rate integrator limit
*
* The portion of the integrator part in the control surface deflection is
* limited to this value
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.05
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.4f);
/**
* Maximum yaw rate
*
@ -311,21 +141,6 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.4f);
*/
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 50.0f);
/**
* Roll control to yaw control feedforward gain.
*
* This gain can be used to counteract the "adverse yaw" effect for fixed wings.
* When the plane enters a roll it will tend to yaw the nose out of the turn.
* This gain enables the use of a yaw actuator (rudder, airbrakes, ...) to counteract
* this effect.
*
* @min 0.0
* @decimal 1
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f);
/**
* Enable wheel steering controller
*
@ -397,50 +212,6 @@ PARAM_DEFINE_FLOAT(FW_WR_IMAX, 0.4f);
*/
PARAM_DEFINE_FLOAT(FW_W_RMAX, 30.0f);
/**
* Roll rate feed forward
*
* Direct feed forward from rate setpoint to control surface output. Use this
* to obtain a tigher response of the controller without introducing
* noise amplification.
*
* @unit %/rad/s
* @min 0.0
* @max 1
* @decimal 2
* @increment 0.05
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f);
/**
* Pitch rate feed forward
*
* Direct feed forward from rate setpoint to control surface output
*
* @unit %/rad/s
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.05
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f);
/**
* Yaw rate feed forward
*
* Direct feed forward from rate setpoint to control surface output
*
* @unit %/rad/s
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.05
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
/**
* Wheel steering rate feed forward
*
@ -471,344 +242,6 @@ PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f);
*/
PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
/**
* Maximum manual roll angle
*
* Maximum manual roll angle setpoint (positive & negative) in manual attitude-only stabilized mode
*
* @unit deg
* @min 0.0
* @max 90.0
* @decimal 1
* @increment 0.5
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
/**
* Maximum manual pitch angle
*
* Maximum manual pitch angle setpoint (positive & negative) in manual attitude-only stabilized mode
*
* @unit deg
* @min 0.0
* @max 90.0
* @decimal 1
* @increment 0.5
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 30.0f);
/**
* Flaps setting during take-off
*
* Sets a fraction of full flaps during take-off.
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f);
/**
* Flaps setting during landing
*
* Sets a fraction of full flaps during landing.
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f);
/**
* Airspeed mode
*
* For small wings or VTOL without airspeed sensor this parameter can be used to
* enable flying without an airspeed reading
*
* @value 0 Normal (use airspeed if available)
* @value 1 Airspeed disabled
* @group FW Attitude Control
*/
PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);
/**
* Enable airspeed scaling
*
* This enables a logic that automatically adjusts the output of the rate controller to take
* into account the real torque produced by an aerodynamic control surface given
* the current deviation from the trim airspeed (FW_AIRSPD_TRIM).
*
* Enable when using aerodynamic control surfaces (e.g.: plane)
* Disable when using rotor wings (e.g.: autogyro)
*
* @boolean
* @group FW Attitude Control
*/
PARAM_DEFINE_INT32(FW_ARSP_SCALE_EN, 1);
/**
* Manual roll scale
*
* Scale factor applied to the desired roll actuator command in full manual mode. This parameter allows
* to adjust the throws of the control surfaces.
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_R_SC, 1.0f);
/**
* Manual pitch scale
*
* Scale factor applied to the desired pitch actuator command in full manual mode. This parameter allows
* to adjust the throws of the control surfaces.
*
* @unit norm
* @min 0.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f);
/**
* Manual yaw scale
*
* Scale factor applied to the desired yaw actuator command in full manual mode. This parameter allows
* to adjust the throws of the control surfaces.
*
* @unit norm
* @min 0.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f);
/**
* Whether to scale throttle by battery power level
*
* This compensates for voltage drop of the battery over time by attempting to
* normalize performance across the operating range of the battery. The fixed wing
* should constantly behave as if it was fully charged with reduced max thrust
* at lower battery percentages. i.e. if cruise speed is at 0.5 throttle at 100% battery,
* it will still be 0.5 at 60% battery.
*
* @boolean
* @group FW Attitude Control
*/
PARAM_DEFINE_INT32(FW_BAT_SCALE_EN, 0);
/**
* Acro body x max rate.
*
* This is the rate the controller is trying to achieve if the user applies full roll
* stick input in acro mode.
*
* @min 45
* @max 720
* @decimal 1
* @increment 5
* @unit deg/s
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90.f);
/**
* Acro body y max rate.
*
* This is the body y rate the controller is trying to achieve if the user applies full pitch
* stick input in acro mode.
*
* @min 45
* @max 720
* @decimal 1
* @increment 5
* @unit deg/s
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90.f);
/**
* Acro body z max rate.
*
* This is the body z rate the controller is trying to achieve if the user applies full yaw
* stick input in acro mode.
*
* @min 10
* @max 180
* @decimal 1
* @increment 5
* @unit deg/s
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45.f);
/**
* Roll trim increment at minimum airspeed
*
* This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MIN.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMIN, 0.0f);
/**
* Pitch trim increment at minimum airspeed
*
* This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MIN.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMIN, 0.0f);
/**
* Yaw trim increment at minimum airspeed
*
* This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MIN.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMIN, 0.0f);
/**
* Roll trim increment at maximum airspeed
*
* This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MAX.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMAX, 0.0f);
/**
* Pitch trim increment at maximum airspeed
*
* This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MAX.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMAX, 0.0f);
/**
* Yaw trim increment at maximum airspeed
*
* This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MAX.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMAX, 0.0f);
/**
* Roll trim increment for flaps configuration
*
* This increment is added to TRIM_ROLL whenever flaps are fully deployed.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_R_FLPS, 0.0f);
/**
* Pitch trim increment for flaps configuration
*
* This increment is added to the pitch trim whenever flaps are fully deployed.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_FLPS, 0.0f);
/**
* Pitch trim increment for spoiler configuration
*
* This increment is added to the pitch trim whenever spoilers are fully deployed.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_SPOIL, 0.f);
/**
* Spoiler landing setting
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f);
/**
* Spoiler descend setting
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f);
/**
* Spoiler input in manual flight
*
* Chose source for manual setting of spoilers in manual flight modes.
*
* @value 0 Disabled
* @value 1 Flaps channel
* @value 2 Aux1
* @group FW Attitude Control
*/
PARAM_DEFINE_INT32(FW_SPOILERS_MAN, 0);
/**
* Maximum manually added yaw rate
*

5
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -2183,11 +2183,6 @@ FixedwingPositionControl::Run()
}
_position_setpoint_current_valid = valid_setpoint;
if (!valid_setpoint) {
events::send(events::ID("fixedwing_position_control_invalid_offboard_sp"), events::Log::Error,
"Invalid offboard setpoint");
}
}
} else {

69
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -514,75 +514,6 @@ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f);
*
*/
/**
* Minimum Airspeed (CAS)
*
* The minimal airspeed (calibrated airspeed) the user is able to command.
* Further, if the airspeed falls below this value, the TECS controller will try to
* increase airspeed more aggressively.
* Has to be set according to the vehicle's stall speed (which should be set in FW_AIRSPD_STALL),
* with some margin between the stall speed and minimum airspeed.
* This value corresponds to the desired minimum speed with the default load factor (level flight, default weight),
* and is automatically adpated to the current load factor (calculated from roll setpoint and WEIGHT_GROSS/WEIGHT_BASE).
*
* @unit m/s
* @min 0.5
* @max 40
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
/**
* Maximum Airspeed (CAS)
*
* The maximal airspeed (calibrated airspeed) the user is able to command.
* Further, if the airspeed is above this value, the TECS controller will try to decrease
* airspeed more aggressively.
*
* @unit m/s
* @min 0.5
* @max 40
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
/**
* Cruise Airspeed (CAS)
*
* The trim CAS (calibrated airspeed) of the vehicle. If an airspeed controller is active,
* this is the default airspeed setpoint that the controller will try to achieve if
* no other airspeed setpoint sources are present (e.g. through non-centered RC sticks).
*
* @unit m/s
* @min 0.5
* @max 40
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
/**
* Stall Airspeed (CAS)
*
* The stall airspeed (calibrated airspeed) of the vehicle.
* It is used for airspeed sensor failure detection and for the control
* surface scaling airspeed limits.
*
* @unit m/s
* @min 0.5
* @max 40
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_STALL, 7.0f);
/**
* Maximum climb rate
*

44
src/modules/fw_rate_control/CMakeLists.txt Normal file
View File

@ -0,0 +1,44 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE modules__fw_rate_control
MAIN fw_rate_control
SRCS
FixedwingRateControl.cpp
FixedwingRateControl.hpp
DEPENDS
px4_work_queue
RateControl
SlewRate
)

624
src/modules/fw_rate_control/FixedwingRateControl.cpp Normal file
View File

@ -0,0 +1,624 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "FixedwingRateControl.hpp"
using namespace time_literals;
using namespace matrix;
using math::constrain;
using math::interpolate;
using math::radians;
FixedwingRateControl::FixedwingRateControl(bool vtol) :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
_actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)),
_actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
/* fetch initial parameter values */
parameters_update();
_rate_ctrl_status_pub.advertise();
_spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate);
_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate);
}
FixedwingRateControl::~FixedwingRateControl()
{
perf_free(_loop_perf);
}
bool
FixedwingRateControl::init()
{
if (!_vehicle_angular_velocity_sub.registerCallback()) {
PX4_ERR("callback registration failed");
return false;
}
return true;
}
int
FixedwingRateControl::parameters_update()
{
const Vector3f rate_p = Vector3f(_param_fw_rr_p.get(), _param_fw_pr_p.get(), _param_fw_yr_p.get());
const Vector3f rate_i = Vector3f(_param_fw_rr_i.get(), _param_fw_pr_i.get(), _param_fw_yr_i.get());
const Vector3f rate_d = Vector3f(_param_fw_rr_d.get(), _param_fw_pr_d.get(), _param_fw_yr_d.get());
_rate_control.setGains(rate_p, rate_i, rate_d);
_rate_control.setIntegratorLimit(
Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get()));
_rate_control.setFeedForwardGain(
// set FF gains to 0 as we add the FF control outside of the rate controller
Vector3f(0.f, 0.f, 0.f));
return PX4_OK;
}
void
FixedwingRateControl::vehicle_manual_poll()
{
if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_climb_rate_enabled
&& _in_fw_or_transition_wo_tailsitter_transition) {
// Always copy the new manual setpoint, even if it wasn't updated, to fill the actuators with valid values
if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) {
if (_vcontrol_mode.flag_control_rates_enabled &&
!_vcontrol_mode.flag_control_attitude_enabled) {
// RATE mode we need to generate the rate setpoint from manual user inputs
_rates_sp.timestamp = hrt_absolute_time();
_rates_sp.roll = _manual_control_setpoint.roll * radians(_param_fw_acro_x_max.get());
_rates_sp.pitch = -_manual_control_setpoint.pitch * radians(_param_fw_acro_y_max.get());
_rates_sp.yaw = _manual_control_setpoint.yaw * radians(_param_fw_acro_z_max.get());
_rates_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;
_rate_sp_pub.publish(_rates_sp);
} else {
/* manual/direct control */
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get();
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
-_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
_actuator_controls.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = (_manual_control_setpoint.throttle + 1.f) * .5f;
}
}
}
}
void
FixedwingRateControl::vehicle_land_detected_poll()
{
if (_vehicle_land_detected_sub.updated()) {
vehicle_land_detected_s vehicle_land_detected {};
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
_landed = vehicle_land_detected.landed;
}
}
}
float FixedwingRateControl::get_airspeed_and_update_scaling()
{
_airspeed_validated_sub.update();
const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().calibrated_airspeed_m_s)
&& (hrt_elapsed_time(&_airspeed_validated_sub.get().timestamp) < 1_s);
// if no airspeed measurement is available out best guess is to use the trim airspeed
float airspeed = _param_fw_airspd_trim.get();
if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed_validated_sub.get().calibrated_airspeed_m_s);
} else {
// VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible
// this assumption is good as long as the vehicle is not hovering in a headwind which is much larger
// than the stall airspeed
if (_vehicle_status.is_vtol && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& !_vehicle_status.in_transition_mode) {
airspeed = _param_fw_airspd_stall.get();
}
}
/*
* For scaling our actuators using anything less than the stall
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and it's the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_stall.get(),
_param_fw_airspd_max.get()), 0.1f, 1000.0f);
_airspeed_scaling = (_param_fw_arsp_scale_en.get()) ? (_param_fw_airspd_trim.get() / airspeed_constrained) : 1.0f;
return airspeed;
}
void FixedwingRateControl::Run()
{
if (should_exit()) {
_vehicle_angular_velocity_sub.unregisterCallback();
exit_and_cleanup();
return;
}
perf_begin(_loop_perf);
// only run controller if angular velocity changed
if (_vehicle_angular_velocity_sub.updated() || (hrt_elapsed_time(&_last_run) > 20_ms)) { //TODO rate!
// only update parameters if they changed
bool params_updated = _parameter_update_sub.updated();
// check for parameter updates
if (params_updated) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
updateParams();
parameters_update();
}
float dt = 0.f;
static constexpr float DT_MIN = 0.002f;
static constexpr float DT_MAX = 0.04f;
vehicle_angular_velocity_s vehicle_angular_velocity{};
if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) {
dt = math::constrain((vehicle_angular_velocity.timestamp_sample - _last_run) * 1e-6f, DT_MIN, DT_MAX);
_last_run = vehicle_angular_velocity.timestamp_sample;
}
if (dt < DT_MIN || dt > DT_MAX) {
const hrt_abstime time_now_us = hrt_absolute_time();
dt = math::constrain((time_now_us - _last_run) * 1e-6f, DT_MIN, DT_MAX);
_last_run = time_now_us;
}
vehicle_angular_velocity_s angular_velocity{};
_vehicle_angular_velocity_sub.copy(&angular_velocity);
float rollspeed = angular_velocity.xyz[0];
float pitchspeed = angular_velocity.xyz[1];
float yawspeed = angular_velocity.xyz[2];
const Vector3f rates(rollspeed, pitchspeed, yawspeed);
const Vector3f angular_accel{angular_velocity.xyz_derivative};
if (_vehicle_status.is_vtol_tailsitter) {
/* roll- and yawspeed have to be swaped */
float helper = rollspeed;
rollspeed = -yawspeed;
yawspeed = helper;
}
// this is only to pass through flaps/spoiler setpoints, can be removed once flaps/spoilers
// are handled outside of attitude/rate controller.
// TODO remove it
_att_sp_sub.update(&_att_sp);
// vehicle status update must be before the vehicle_control_mode poll, otherwise rate sp are not published during whole transition
_vehicle_status_sub.update(&_vehicle_status);
const bool is_in_transition_except_tailsitter = _vehicle_status.in_transition_mode
&& !_vehicle_status.is_vtol_tailsitter;
const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
_in_fw_or_transition_wo_tailsitter_transition = is_fixed_wing || is_in_transition_except_tailsitter;
_vehicle_control_mode_sub.update(&_vcontrol_mode);
vehicle_land_detected_poll();
vehicle_manual_poll();
vehicle_land_detected_poll();
/* if we are in rotary wing mode, do nothing */
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) {
_spoiler_setpoint_with_slewrate.setForcedValue(0.f);
_flaps_setpoint_with_slewrate.setForcedValue(0.f);
perf_end(_loop_perf);
return;
}
controlFlaps(dt);
controlSpoilers(dt);
if (_vcontrol_mode.flag_control_rates_enabled) {
const float airspeed = get_airspeed_and_update_scaling();
/* reset integrals where needed */
if (_rates_sp.reset_integral) {
_rate_control.resetIntegral();
}
// Reset integrators if the aircraft is on ground or not in a state where the fw attitude controller is run
if (_landed || !_in_fw_or_transition_wo_tailsitter_transition) {
_rate_control.resetIntegral();
}
// update saturation status from control allocation feedback
control_allocator_status_s control_allocator_status;
if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) {
Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative;
if (!control_allocator_status.torque_setpoint_achieved) {
for (size_t i = 0; i < 3; i++) {
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
saturation_positive(i) = true;
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true;
}
}
}
// TODO: send the unallocated value directly for better anti-windup
_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
}
/* bi-linear interpolation over airspeed for actuator trim scheduling */
float trim_roll = _param_trim_roll.get();
float trim_pitch = _param_trim_pitch.get();
float trim_yaw = _param_trim_yaw.get();
if (airspeed < _param_fw_airspd_trim.get()) {
trim_roll += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
_param_fw_dtrim_r_vmin.get(),
0.0f);
trim_pitch += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
_param_fw_dtrim_p_vmin.get(),
0.0f);
trim_yaw += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
_param_fw_dtrim_y_vmin.get(),
0.0f);
} else {
trim_roll += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_param_fw_dtrim_r_vmax.get());
trim_pitch += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_param_fw_dtrim_p_vmax.get());
trim_yaw += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_param_fw_dtrim_y_vmax.get());
}
/* add trim increment if flaps are deployed */
trim_roll += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_r_flps.get();
trim_pitch += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_flps.get();
// add trim increment from spoilers (only pitch)
trim_pitch += _spoiler_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_spoil.get();
if (_vcontrol_mode.flag_control_rates_enabled) {
_rates_sp_sub.update(&_rates_sp);
const Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw);
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt,
_landed);
float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0);
float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
(PX4_ISFINITE(roll_u)) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll;
float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1);
float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
(PX4_ISFINITE(pitch_u)) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch;
const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2);
float yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw,
-1.f, 1.f) : trim_yaw;
if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) {
_rate_control.resetIntegral();
}
/* throttle passed through if it is finite */
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] =
(PX4_ISFINITE(_rates_sp.thrust_body[0])) ? _rates_sp.thrust_body[0] : 0.0f;
/* scale effort by battery status */
if (_param_fw_bat_scale_en.get() &&
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
if (_battery_status_sub.updated()) {
battery_status_s battery_status{};
if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
_battery_scale = battery_status.scale;
}
}
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
}
}
// publish rate controller status
rate_ctrl_status_s rate_ctrl_status{};
_rate_control.getRateControlStatus(rate_ctrl_status);
rate_ctrl_status.timestamp = hrt_absolute_time();
_rate_ctrl_status_pub.publish(rate_ctrl_status);
} else {
// full manual
_rate_control.resetIntegral();
}
// Add feed-forward from roll control output to yaw control output
// This can be used to counteract the adverse yaw effect when rolling the plane
_actuator_controls.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get()
* constrain(_actuator_controls.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_actuator_controls.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
_actuator_controls.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
_actuator_controls.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
_actuator_controls.control[actuator_controls_s::INDEX_LANDING_GEAR] = _manual_control_setpoint.aux3;
/* lazily publish the setpoint only once available */
_actuator_controls.timestamp = hrt_absolute_time();
_actuator_controls.timestamp_sample = vehicle_angular_velocity.timestamp;
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled) {
_actuator_controls_0_pub.publish(_actuator_controls);
if (!_vehicle_status.is_vtol) {
publishTorqueSetpoint(angular_velocity.timestamp_sample);
publishThrustSetpoint(angular_velocity.timestamp_sample);
}
}
updateActuatorControlsStatus(dt);
}
// backup schedule
ScheduleDelayed(20_ms);
perf_end(_loop_perf);
}
void FixedwingRateControl::publishTorqueSetpoint(const hrt_abstime &timestamp_sample)
{
vehicle_torque_setpoint_s v_torque_sp = {};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.timestamp_sample = timestamp_sample;
v_torque_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_ROLL];
v_torque_sp.xyz[1] = _actuator_controls.control[actuator_controls_s::INDEX_PITCH];
v_torque_sp.xyz[2] = _actuator_controls.control[actuator_controls_s::INDEX_YAW];
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
void FixedwingRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
{
vehicle_thrust_setpoint_s v_thrust_sp = {};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.timestamp_sample = timestamp_sample;
v_thrust_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
void FixedwingRateControl::controlFlaps(const float dt)
{
/* default flaps to center */
float flap_control = 0.0f;
/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled) {
flap_control = _manual_control_setpoint.flaps;
} else if (_vcontrol_mode.flag_control_auto_enabled) {
switch (_att_sp.apply_flaps) {
case vehicle_attitude_setpoint_s::FLAPS_OFF:
flap_control = 0.0f; // no flaps
break;
case vehicle_attitude_setpoint_s::FLAPS_LAND:
flap_control = _param_fw_flaps_lnd_scl.get();
break;
case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF:
flap_control = _param_fw_flaps_to_scl.get();
break;
}
}
// move the actual control value continuous with time, full flap travel in 1sec
_flaps_setpoint_with_slewrate.update(math::constrain(flap_control, 0.f, 1.f), dt);
}
void FixedwingRateControl::controlSpoilers(const float dt)
{
float spoiler_control = 0.f;
if (_vcontrol_mode.flag_control_manual_enabled) {
switch (_param_fw_spoilers_man.get()) {
case 0:
break;
case 1:
spoiler_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? _manual_control_setpoint.flaps : 0.f;
break;
case 2:
spoiler_control = PX4_ISFINITE(_manual_control_setpoint.aux1) ? _manual_control_setpoint.aux1 : 0.f;
break;
}
} else if (_vcontrol_mode.flag_control_auto_enabled) {
switch (_att_sp.apply_spoilers) {
case vehicle_attitude_setpoint_s::SPOILERS_OFF:
spoiler_control = 0.f;
break;
case vehicle_attitude_setpoint_s::SPOILERS_LAND:
spoiler_control = _param_fw_spoilers_lnd.get();
break;
case vehicle_attitude_setpoint_s::SPOILERS_DESCEND:
spoiler_control = _param_fw_spoilers_desc.get();
break;
}
}
_spoiler_setpoint_with_slewrate.update(math::constrain(spoiler_control, 0.f, 1.f), dt);
}
void FixedwingRateControl::updateActuatorControlsStatus(float dt)
{
for (int i = 0; i < 4; i++) {
float control_signal;
if (i <= actuator_controls_status_s::INDEX_YAW) {
// We assume that the attitude is actuated by control surfaces
// consuming power only when they move
control_signal = _actuator_controls.control[i] - _control_prev[i];
_control_prev[i] = _actuator_controls.control[i];
} else {
control_signal = _actuator_controls.control[i];
}
_control_energy[i] += control_signal * control_signal * dt;
}
_energy_integration_time += dt;
if (_energy_integration_time > 500e-3f) {
actuator_controls_status_s status;
status.timestamp = _actuator_controls.timestamp;
for (int i = 0; i < 4; i++) {
status.control_power[i] = _control_energy[i] / _energy_integration_time;
_control_energy[i] = 0.f;
}
_actuator_controls_status_pub.publish(status);
_energy_integration_time = 0.f;
}
}
int FixedwingRateControl::task_spawn(int argc, char *argv[])
{
bool vtol = false;
if (argc > 1) {
if (strcmp(argv[1], "vtol") == 0) {
vtol = true;
}
}
FixedwingRateControl *instance = new FixedwingRateControl(vtol);
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
int FixedwingRateControl::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int FixedwingRateControl::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
fw_rate_control is the fixed-wing rate controller.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("fw_rate_control", "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int fw_rate_control_main(int argc, char *argv[])
{
return FixedwingRateControl::main(argc, argv);
}

245
src/modules/fw_rate_control/FixedwingRateControl.hpp Normal file
View File

@ -0,0 +1,245 @@
/****************************************************************************
*
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <lib/rate_control/rate_control.hpp>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <lib/slew_rate/SlewRate.hpp>
#include <matrix/math.hpp>
#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/tasks.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_status.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_allocator_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_angular_velocity.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_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
using matrix::Eulerf;
using matrix::Quatf;
using uORB::SubscriptionData;
using namespace time_literals;
static constexpr float kFlapSlewRate = 1.f; //minimum time from none to full flap deflection [s]
static constexpr float kSpoilerSlewRate = 1.f; //minimum time from none to full spoiler deflection [s]
class FixedwingRateControl final : public ModuleBase<FixedwingRateControl>, public ModuleParams,
public px4::ScheduledWorkItem
{
public:
FixedwingRateControl(bool vtol = false);
~FixedwingRateControl() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
private:
void Run() override;
void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
uORB::SubscriptionMultiArray<control_allocator_status_s, 2> _control_allocator_status_subs{ORB_ID::control_allocator_status};
uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
uORB::Publication<actuator_controls_s> _actuator_controls_0_pub;
uORB::Publication<actuator_controls_status_s> _actuator_controls_status_pub;
uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
actuator_controls_s _actuator_controls{};
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_attitude_setpoint_s _att_sp{};
vehicle_control_mode_s _vcontrol_mode{};
vehicle_rates_setpoint_s _rates_sp{};
vehicle_status_s _vehicle_status{};
perf_counter_t _loop_perf;
hrt_abstime _last_run{0};
float _airspeed_scaling{1.0f};
bool _landed{true};
float _battery_scale{1.0f};
float _energy_integration_time{0.0f};
float _control_energy[4] {};
float _control_prev[3] {};
SlewRate<float> _spoiler_setpoint_with_slewrate;
SlewRate<float> _flaps_setpoint_with_slewrate;
bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states
DEFINE_PARAMETERS(
(ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max,
(ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max,
(ParamFloat<px4::params::FW_ACRO_Z_MAX>) _param_fw_acro_z_max,
(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
(ParamInt<px4::params::FW_ARSP_SCALE_EN>) _param_fw_arsp_scale_en,
(ParamBool<px4::params::FW_BAT_SCALE_EN>) _param_fw_bat_scale_en,
(ParamFloat<px4::params::FW_DTRIM_P_FLPS>) _param_fw_dtrim_p_flps,
(ParamFloat<px4::params::FW_DTRIM_P_SPOIL>) _param_fw_dtrim_p_spoil,
(ParamFloat<px4::params::FW_DTRIM_P_VMAX>) _param_fw_dtrim_p_vmax,
(ParamFloat<px4::params::FW_DTRIM_P_VMIN>) _param_fw_dtrim_p_vmin,
(ParamFloat<px4::params::FW_DTRIM_R_FLPS>) _param_fw_dtrim_r_flps,
(ParamFloat<px4::params::FW_DTRIM_R_VMAX>) _param_fw_dtrim_r_vmax,
(ParamFloat<px4::params::FW_DTRIM_R_VMIN>) _param_fw_dtrim_r_vmin,
(ParamFloat<px4::params::FW_DTRIM_Y_VMAX>) _param_fw_dtrim_y_vmax,
(ParamFloat<px4::params::FW_DTRIM_Y_VMIN>) _param_fw_dtrim_y_vmin,
(ParamFloat<px4::params::FW_FLAPS_LND_SCL>) _param_fw_flaps_lnd_scl,
(ParamFloat<px4::params::FW_FLAPS_TO_SCL>) _param_fw_flaps_to_scl,
(ParamFloat<px4::params::FW_SPOILERS_LND>) _param_fw_spoilers_lnd,
(ParamFloat<px4::params::FW_SPOILERS_DESC>) _param_fw_spoilers_desc,
(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man,
(ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
(ParamFloat<px4::params::FW_MAN_P_SC>) _param_fw_man_p_sc,
(ParamFloat<px4::params::FW_MAN_R_MAX>) _param_fw_man_r_max,
(ParamFloat<px4::params::FW_MAN_R_SC>) _param_fw_man_r_sc,
(ParamFloat<px4::params::FW_MAN_Y_SC>) _param_fw_man_y_sc,
(ParamFloat<px4::params::FW_PR_FF>) _param_fw_pr_ff,
(ParamFloat<px4::params::FW_PR_I>) _param_fw_pr_i,
(ParamFloat<px4::params::FW_PR_IMAX>) _param_fw_pr_imax,
(ParamFloat<px4::params::FW_PR_P>) _param_fw_pr_p,
(ParamFloat<px4::params::FW_PR_D>) _param_fw_pr_d,
(ParamFloat<px4::params::FW_RLL_TO_YAW_FF>) _param_fw_rll_to_yaw_ff,
(ParamFloat<px4::params::FW_RR_FF>) _param_fw_rr_ff,
(ParamFloat<px4::params::FW_RR_I>) _param_fw_rr_i,
(ParamFloat<px4::params::FW_RR_IMAX>) _param_fw_rr_imax,
(ParamFloat<px4::params::FW_RR_P>) _param_fw_rr_p,
(ParamFloat<px4::params::FW_RR_D>) _param_fw_rr_d,
(ParamFloat<px4::params::FW_YR_FF>) _param_fw_yr_ff,
(ParamFloat<px4::params::FW_YR_I>) _param_fw_yr_i,
(ParamFloat<px4::params::FW_YR_IMAX>) _param_fw_yr_imax,
(ParamFloat<px4::params::FW_YR_P>) _param_fw_yr_p,
(ParamFloat<px4::params::FW_YR_D>) _param_fw_yr_d,
(ParamFloat<px4::params::TRIM_PITCH>) _param_trim_pitch,
(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw
)
RateControl _rate_control; ///< class for rate control calculations
/**
* @brief Update flap control setting
*
* @param dt Current time delta [s]
*/
void controlFlaps(const float dt);
/**
* @brief Update spoiler control setting
*
* @param dt Current time delta [s]
*/
void controlSpoilers(const float dt);
void updateActuatorControlsStatus(float dt);
/**
* Update our local parameter cache.
*/
int parameters_update();
void vehicle_manual_poll();
void vehicle_land_detected_poll();
float get_airspeed_and_update_scaling();
};

12
src/modules/fw_rate_control/Kconfig Normal file
View File

@ -0,0 +1,12 @@
menuconfig MODULES_FW_RATE_CONTROL
bool "fw_rate_control"
default n
---help---
Enable support for fw_att_control
menuconfig USER_FW_RATE_CONTROL
bool "fw_rate_control running as userspace module"
default n
depends on BOARD_PROTECTED && MODULES_FW_RATE_CONTROL
---help---
Put fw_rate_control in userspace memory

677
src/modules/fw_rate_control/fw_rate_control_params.c Normal file
View File

@ -0,0 +1,677 @@
/****************************************************************************
*
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fw_rate_control_params.c
*
* Parameters defined by the fixed-wing attitude control task
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Thomas Gubler <thomas@px4.io>
*/
/*
* Controller parameters, accessible via MAVLink
*
*/
/**
* Minimum Airspeed (CAS)
*
* The minimal airspeed (calibrated airspeed) the user is able to command.
* Further, if the airspeed falls below this value, the TECS controller will try to
* increase airspeed more aggressively.
* Has to be set according to the vehicle's stall speed (which should be set in FW_AIRSPD_STALL),
* with some margin between the stall speed and minimum airspeed.
* This value corresponds to the desired minimum speed with the default load factor (level flight, default weight),
* and is automatically adpated to the current load factor (calculated from roll setpoint and WEIGHT_GROSS/WEIGHT_BASE).
*
* @unit m/s
* @min 0.5
* @max 40
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
/**
* Maximum Airspeed (CAS)
*
* The maximal airspeed (calibrated airspeed) the user is able to command.
* Further, if the airspeed is above this value, the TECS controller will try to decrease
* airspeed more aggressively.
*
* @unit m/s
* @min 0.5
* @max 40
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
/**
* Airspeed mode
*
* For small wings or VTOL without airspeed sensor this parameter can be used to
* enable flying without an airspeed reading
*
* @value 0 Normal (use airspeed if available)
* @value 1 Airspeed disabled
* @group FW Attitude Control
*/
PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);
/**
* Cruise Airspeed (CAS)
*
* The trim CAS (calibrated airspeed) of the vehicle. If an airspeed controller is active,
* this is the default airspeed setpoint that the controller will try to achieve if
* no other airspeed setpoint sources are present (e.g. through non-centered RC sticks).
*
* @unit m/s
* @min 0.5
* @max 40
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
/**
* Stall Airspeed (CAS)
*
* The stall airspeed (calibrated airspeed) of the vehicle.
* It is used for airspeed sensor failure detection and for the control
* surface scaling airspeed limits.
*
* @unit m/s
* @min 0.5
* @max 40
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_STALL, 7.0f);
/**
* Pitch rate proportional gain.
*
* Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @unit %/rad/s
* @min 0.0
* @max 1.0
* @decimal 3
* @increment 0.005
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f);
/**
* Pitch rate derivative gain.
*
* Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @unit %/rad/s
* @min 0.0
* @max 1.0
* @decimal 3
* @increment 0.005
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_PR_D, 0.f);
/**
* Pitch rate integrator gain.
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @unit %/rad
* @min 0.0
* @max 0.5
* @decimal 3
* @increment 0.005
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_PR_I, 0.1f);
/**
* Pitch rate integrator limit
*
* The portion of the integrator part in the control surface deflection is
* limited to this value
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.05
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f);
/**
* Roll rate proportional Gain
*
* Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @unit %/rad/s
* @min 0.0
* @max 1.0
* @decimal 3
* @increment 0.005
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
/**
* Roll rate derivative Gain
*
* Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @unit %/rad/s
* @min 0.0
* @max 1.0
* @decimal 3
* @increment 0.005
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_RR_D, 0.00f);
/**
* Roll rate integrator Gain
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @unit %/rad
* @min 0.0
* @max 0.2
* @decimal 3
* @increment 0.005
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_RR_I, 0.1f);
/**
* Roll integrator anti-windup
*
* The portion of the integrator part in the control surface deflection is limited to this value.
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.05
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
/**
* Yaw rate proportional gain
*
* Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @unit %/rad/s
* @min 0.0
* @max 1.0
* @decimal 3
* @increment 0.005
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
/**
* Yaw rate derivative gain
*
* Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @unit %/rad/s
* @min 0.0
* @max 1.0
* @decimal 3
* @increment 0.005
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_YR_D, 0.0f);
/**
* Yaw rate integrator gain
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @unit %/rad
* @min 0.0
* @max 50.0
* @decimal 1
* @increment 0.5
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_YR_I, 0.1f);
/**
* Yaw rate integrator limit
*
* The portion of the integrator part in the control surface deflection is
* limited to this value
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.05
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
/**
* Roll rate feed forward
*
* Direct feed forward from rate setpoint to control surface output. Use this
* to obtain a tigher response of the controller without introducing
* noise amplification.
*
* @unit %/rad/s
* @min 0.0
* @max 10.0
* @decimal 2
* @increment 0.05
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f);
/**
* Pitch rate feed forward
*
* Direct feed forward from rate setpoint to control surface output
*
* @unit %/rad/s
* @min 0.0
* @max 10.0
* @decimal 2
* @increment 0.05
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f);
/**
* Yaw rate feed forward
*
* Direct feed forward from rate setpoint to control surface output
*
* @unit %/rad/s
* @min 0.0
* @max 10.0
* @decimal 2
* @increment 0.05
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
/**
* Acro body x max rate.
*
* This is the rate the controller is trying to achieve if the user applies full roll
* stick input in acro mode.
*
* @min 45
* @max 720
* @unit deg
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90);
/**
* Acro body y max rate.
*
* This is the body y rate the controller is trying to achieve if the user applies full pitch
* stick input in acro mode.
*
* @min 45
* @max 720
* @unit deg
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90);
/**
* Acro body z max rate.
*
* This is the body z rate the controller is trying to achieve if the user applies full yaw
* stick input in acro mode.
*
* @min 10
* @max 180
* @unit deg
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45);
/**
* Whether to scale throttle by battery power level
*
* This compensates for voltage drop of the battery over time by attempting to
* normalize performance across the operating range of the battery. The fixed wing
* should constantly behave as if it was fully charged with reduced max thrust
* at lower battery percentages. i.e. if cruise speed is at 0.5 throttle at 100% battery,
* it will still be 0.5 at 60% battery.
*
* @boolean
* @group FW Rate Control
*/
PARAM_DEFINE_INT32(FW_BAT_SCALE_EN, 0);
/**
* Enable airspeed scaling
*
* This enables a logic that automatically adjusts the output of the rate controller to take
* into account the real torque produced by an aerodynamic control surface given
* the current deviation from the trim airspeed (FW_AIRSPD_TRIM).
*
* Enable when using aerodynamic control surfaces (e.g.: plane)
* Disable when using rotor wings (e.g.: autogyro)
*
* @boolean
* @group FW Rate Control
*/
PARAM_DEFINE_INT32(FW_ARSP_SCALE_EN, 1);
/**
* Roll trim increment at minimum airspeed
*
* This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MIN.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMIN, 0.0f);
/**
* Pitch trim increment at minimum airspeed
*
* This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MIN.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMIN, 0.0f);
/**
* Yaw trim increment at minimum airspeed
*
* This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MIN.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMIN, 0.0f);
/**
* Roll trim increment at maximum airspeed
*
* This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MAX.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMAX, 0.0f);
/**
* Pitch trim increment at maximum airspeed
*
* This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MAX.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMAX, 0.0f);
/**
* Yaw trim increment at maximum airspeed
*
* This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MAX.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMAX, 0.0f);
/**
* Roll trim increment for flaps configuration
*
* This increment is added to TRIM_ROLL whenever flaps are fully deployed.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_R_FLPS, 0.0f);
/**
* Pitch trim increment for flaps configuration
*
* This increment is added to the pitch trim whenever flaps are fully deployed.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_FLPS, 0.0f);
/**
* Pitch trim increment for spoiler configuration
*
* This increment is added to the pitch trim whenever spoilers are fully deployed.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_SPOIL, 0.f);
/**
* Flaps setting during take-off
*
* Sets a fraction of full flaps during take-off.
* Also applies to flaperons if enabled in the mixer/allocation.
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f);
/**
* Flaps setting during landing
*
* Sets a fraction of full flaps during landing.
* Also applies to flaperons if enabled in the mixer/allocation.
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f);
/**
* Manual roll scale
*
* Scale factor applied to the desired roll actuator command in full manual mode. This parameter allows
* to adjust the throws of the control surfaces.
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_R_SC, 1.0f);
/**
* Maximum manual pitch angle
*
* Maximum manual pitch angle setpoint (positive & negative) in manual attitude-only stabilized mode
*
* @unit deg
* @min 0.0
* @max 90.0
* @decimal 1
* @increment 0.5
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 30.0f);
/**
* Manual pitch scale
*
* Scale factor applied to the desired pitch actuator command in full manual mode. This parameter allows
* to adjust the throws of the control surfaces.
*
* @unit norm
* @min 0.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f);
/**
* Maximum manual roll angle
*
* Maximum manual roll angle setpoint (positive & negative) in manual attitude-only stabilized mode
*
* @unit deg
* @min 0.0
* @max 90.0
* @decimal 1
* @increment 0.5
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
/**
* Manual yaw scale
*
* Scale factor applied to the desired yaw actuator command in full manual mode. This parameter allows
* to adjust the throws of the control surfaces.
*
* @unit norm
* @min 0.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f);
/**
* Roll control to yaw control feedforward gain.
*
* This gain can be used to counteract the "adverse yaw" effect for fixed wings.
* When the plane enters a roll it will tend to yaw the nose out of the turn.
* This gain enables the use of a yaw actuator (rudder, airbrakes, ...) to counteract
* this effect.
*
* @min 0.0
* @decimal 1
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f);
/**
* Spoiler landing setting
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f);
/**
* Spoiler descend setting
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f);
/**
* Spoiler input in manual flight
*
* Chose source for manual setting of spoilers in manual flight modes.
*
* @value 0 Disabled
* @value 1 Flaps channel
* @value 2 Aux1
* @group FW Attitude Control
*/
PARAM_DEFINE_INT32(FW_SPOILERS_MAN, 0);