PX4二次开发——PX4程序架构
PX4程序架构
一、从RCS启动脚本可以看出哪些东西
启动脚本是一个神奇的东西，它能够识别出你对应的飞机类型，加载对应的混控器，选择对应的姿态、位置估计程序以及控制程序，初始化你需要的驱动程序。下面来分析下。
1.1RCS位置
- Firmware->ROMFS->px4fmu_common->init.d->rcs，最新版本的启动脚本如下：
#!/bin/sh
# Un comment and use set +e to ignore and set -e to enable 'exit on error control'
set +e
# Un comment the line below to help debug scripts by printing a trace of the script commands
#set -x
# PX4FMU startup script.
#
# NOTE: environment variable references:
# If the dollar sign ('$') is followed by a left bracket ('{') then the
# variable name is terminated with the right bracket character ('}').
# Otherwise, the variable name goes to the end of the argument.
#
#
# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
#
#------------------------------------------------------------------------------
#
# Set default paramter values.
# Do not add intra word spaces
# it wastes flash
#
set R /
set AUTOCNF no
set AUX_MODE pwm
set DATAMAN_OPT ""
set FAILSAFE none
set FAILSAFE_AUX none
set FCONFIG /fs/microsd/etc/config.txt
set FEXTRAS /fs/microsd/etc/extras.txt
set FMU_MODE pwm
set FRC /fs/microsd/etc/rc.txt
set IOFW "/etc/extras/px4_io-v2_default.bin"
set IO_PRESENT no
set LOG_FILE /fs/microsd/bootlog.txt
set LOGGER_ARGS ""
set LOGGER_BUF 14
set MAV_TYPE none
set MIXER none
set MIXER_AUX none
set MIXER_FILE none
set MK_MODE none
set MKBLCTRL_ARG ""
set OUTPUT_MODE none
set PARAM_FILE /fs/microsd/params
set PWM_AUX_DISARMED p:PWM_AUX_DISARMED
set PWM_AUX_MAX p:PWM_AUX_MAX
set PWM_AUX_MIN p:PWM_AUX_MIN
set PWM_AUX_OUT none
set PWM_AUX_RATE p:PWM_AUX_RATE
set PWM_DISARMED p:PWM_DISARMED
set PWM_MAX p:PWM_MAX
set PWM_MIN p:PWM_MIN
set PWM_OUT none
set PWM_RATE p:PWM_RATE
set RC_INPUT_ARGS ""
set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers
set STARTUP_TUNE 1
set USE_IO no
set VEHICLE_TYPE none
# Airframe parameter versioning: airframe maintainers can set this in the
# airframe startup script, and then increase it by one whenever an airframe
# parameter is updated - it will ensure that these parameters will be updated
# when the firmware is flashed.
set PARAM_DEFAULTS_VER 1
#
# Mount the procfs.
#
mount -t procfs /proc
#
# Start CDC/ACM serial driver.
#
sercon
#
# Print full system version.
#
ver all
#
# Start the ORB (first app to start)
# tone_alarm and tune_control
# is dependent.
#
uorb start
#
# Try to mount the microSD card.
#
# REBOOTWORK this needs to start after the flight control loop.
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
if hardfault_log check
then
# Error tune.
set STARTUP_TUNE 2
if hardfault_log commit
then
hardfault_log reset
fi
fi
# Prevent MacOS and Ubuntu from creating unnecessary temporary files on the microSD card
# block MacOS Spotlight indexing (.Spotlight-V100 folder)
if [ ! -f "/fs/microsd/.metadata_never_index" ]; then
cat > /fs/microsd/.metadata_never_index
fi
# block MacOS trashes
if [ ! -f "/fs/microsd/.Trashes" ]; then
cat > /fs/microsd/.Trashes
fi
# block MacOS logging of filesystem events
if [ ! -d "/fs/microsd/.fseventsd" ]; then
mkdir /fs/microsd/.fseventsd
fi
if [ ! -f "/fs/microsd/.fseventsd/no_log" ]; then
cat > /fs/microsd/.fseventsd/no_log
fi
# block Ubuntu trash
if [ ! -f "/fs/microsd/.Trash-1000" ]; then
cat > /fs/microsd/.Trash-1000
fi
else
# tune SD_INIT
set STARTUP_TUNE 14 # tune 14 = SD_INIT
if mkfatfs /dev/mmcsd0
then
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
echo "INFO [init] card formatted"
else
set STARTUP_TUNE 15 # tune 15 = SD_ERROR
echo "ERROR [init] format failed"
set LOG_FILE /dev/null
fi
else
set LOG_FILE /dev/null
fi
fi
#
# Look for an init script on the microSD card.
# Disable autostart if the script found.
#
if [ -f $FRC ]
then
. $FRC
else
#
# Set the parameter file if mtd starts successfully.
#
if mtd start
then
set PARAM_FILE /fs/mtd_params
fi
#
# Load parameters.
#
# if MTD has a secondary storage it is used for (factory) calibration data
if mtd has-secondary
then
if mtd start -i 1 /fs/mtd_caldata
then
param load /fs/mtd_caldata
fi
fi
param select $PARAM_FILE
if ! param import
then
param reset_all
fi
#
# Set AUTOCNF flag to use it in AUTOSTART scripts.
#
if param greater SYS_AUTOCONFIG 0
then
if param compare SYS_AUTOCONFIG 1
then
# Wipe out params except RC*, flight modes, total flight time, accel cal, gyro cal, next flight UUID
param reset_all SYS_AUTOSTART SYS_AUTOCONFIG RC* COM_FLTMODE* LND_FLIGHT_T_* TC_* CAL_ACC* CAL_GYRO* COM_FLIGHT_UUID
fi
set AUTOCNF yes
fi
#
# Optional board defaults: rc.board_defaults
#
set BOARD_RC_DEFAULTS ${R}etc/init.d/rc.board_defaults
if [ -f $BOARD_RC_DEFAULTS ]
then
echo "Board defaults: ${BOARD_RC_DEFAULTS}"
. $BOARD_RC_DEFAULTS
fi
unset BOARD_RC_DEFAULTS
#
# Start the tone_alarm driver.
# Needs to be started after the parameters are loaded (for CBRK_BUZZER).
#
tone_alarm start
#
# Play the startup tune (if not disabled or there is an error)
#
param compare CBRK_BUZZER 782090
if [ $? != 0 -o $STARTUP_TUNE != 1 ]
then
tune_control play -t $STARTUP_TUNE
fi
#
# Waypoint storage.
# REBOOTWORK this needs to start in parallel.
#
dataman start $DATAMAN_OPT
#
# Start the socket communication send_event handler.
#
send_event start
#
# Start the resource load monitor.
#
load_mon start
#
# Start system state indicator.
#
rgbled start -X -q
rgbled_ncp5623c start -X -q
if param greater LIGHT_EN_BLINKM 0
then
if blinkm start -X
then
blinkm systemstate
fi
fi
#
# Set parameters and env variables for selected AUTOSTART.
#
if ! param compare SYS_AUTOSTART 0
then
. ${R}etc/init.d/rc.autostart
fi
#
# Override parameters from user configuration file.
#
if [ -f $FCONFIG ]
then
echo "Custom: ${FCONFIG}"
. $FCONFIG
fi
#
# If autoconfig parameter was set, reset it and save parameters.
#
if [ $AUTOCNF = yes ]
then
param set SYS_AUTOCONFIG 0
fi
#
# Check if PX4IO present and update firmware if needed.
# Assumption IOFW set to firmware file and IO_PRESENT = no
#
if [ -f $IOFW ]
then
# Check for the mini using build with px4io fw file
# but not a px4IO
if ver hwtypecmp V540 V560
then
param set SYS_USE_IO 0
else
if px4io checkcrc ${IOFW}
then
set IO_PRESENT yes
else
# tune Program PX4IO
tune_control play -t 16 # tune 16 = PROG_PX4IO
if px4io start
then
# Try to safety px4 io so motor outputs don't go crazy.
if ! px4io safety_on
then
# px4io did not respond to the safety command.
px4io stop
fi
fi
if px4io forceupdate 14662 ${IOFW}
then
usleep 10000
tune_control stop
if px4io checkcrc ${IOFW}
then
echo "PX4IO CRC OK after updating" >> $LOG_FILE
tune_control play -t 17 # tune 17 = PROG_PX4IO_OK
set IO_PRESENT yes
fi
fi
if [ $IO_PRESENT = no ]
then
echo "PX4IO update failed" >> $LOG_FILE
tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR
fi
fi
fi
fi
#
# Set USE_IO flag.
#
if param compare -s SYS_USE_IO 1
then
set USE_IO yes
fi
if [ $USE_IO = yes -a $IO_PRESENT = no ]
then
echo "PX4IO not found" >> $LOG_FILE
tune_control play error
fi
#
# RC update (map raw RC input to calibrate manual control)
# start before commander
#
rc_update start
#
# Sensors System (start before Commander so Preflight checks are properly run).
# Commander needs to be this early for in-air-restarts.
#
if param greater SYS_HITL 0
then
set OUTPUT_MODE hil
sensors start -h
commander start -h
# disable GPS
param set GPS_1_CONFIG 0
# start the simulator in hardware if needed
if param compare SYS_HITL 2
then
sih start
fi
else
#
# board sensors: rc.sensors
#
set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors
if [ -f $BOARD_RC_SENSORS ]
then
echo "Board sensors: ${BOARD_RC_SENSORS}"
. $BOARD_RC_SENSORS
fi
unset BOARD_RC_SENSORS
. ${R}etc/init.d/rc.sensors
if param compare -s BAT1_SOURCE 2
then
esc_battery start
fi
if ! param compare BAT1_SOURCE 1
then
battery_status start
fi
commander start
fi
# Sensors on the PWM interface bank.
if param compare -s SENS_EN_LL40LS 1
then
# Clear pins 5 and 6.
set FMU_MODE pwm4
set AUX_MODE pwm4
fi
# Check if ATS is enabled
if param compare FD_EXT_ATS_EN 1
then
# Clear pins 5 and 6.
set FMU_MODE pwm4
set AUX_MODE pwm4
fi
if param greater TRIG_MODE 0
then
# We ONLY support trigger on pins 5+6 or 7+8 when simultanously using AUX for actuator output.
if param compare TRIG_PINS 56
then
# clear pins 5 and 6
set FMU_MODE pwm4
set AUX_MODE pwm4
else
if param compare TRIG_PINS 78
then
# clear pins 7 and 8
set FMU_MODE pwm6
set AUX_MODE pwm6
else
set FMU_MODE none
set AUX_MODE none
fi
fi
camera_trigger start
camera_feedback start
fi
#
# Check if UAVCAN is enabled, default to it for ESCs.
#
if param greater -s UAVCAN_ENABLE 0
then
# Start core UAVCAN module.
if uavcan start
then
if param greater UAVCAN_ENABLE 1
then
# Start UAVCAN firmware update server and dynamic node ID allocation server.
uavcan start fw
if param greater UAVCAN_ENABLE 2
then
set OUTPUT_MODE uavcan_esc
fi
fi
else
tune_control play error
fi
fi
#
# Optional board mavlink streams: rc.board_mavlink
#
set BOARD_RC_MAVLINK ${R}etc/init.d/rc.board_mavlink
if [ -f $BOARD_RC_MAVLINK ]
then
echo "Board extras: ${BOARD_RC_MAVLINK}"
. $BOARD_RC_MAVLINK
fi
unset BOARD_RC_MAVLINK
#
# Start UART/Serial device drivers.
# Note: rc.serial is auto-generated from Tools/serial/generate_config.py
#
. ${R}etc/init.d/rc.serial
if [ $IO_PRESENT = no ]
then
# Must be started after the serial config is read
rc_input start $RC_INPUT_ARGS
fi
#
# Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for rc.interface,
# rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
#
. ${R}etc/init.d/rc.vehicle_setup
# Camera capture driver
if param greater -s CAM_CAP_FBACK 0
then
if camera_capture start
then
camera_capture on
fi
fi
#
# Start the navigator.
#
navigator start
#
# Start a thermal calibration if required.
#
. ${R}etc/init.d/rc.thermal_cal
#
# Start vmount to control mounts such as gimbals, disabled by default.
#
if ! param compare MNT_MODE_IN -1
then
vmount start
fi
# Check for flow sensor
if param compare SENS_EN_PX4FLOW 1
then
px4flow start -X
fi
# Blacksheep telemetry
if param greater TEL_BST_EN 0
then
bst start -X
fi
#
# Optional board supplied extras: rc.board_extras
#
set BOARD_RC_EXTRAS ${R}etc/init.d/rc.board_extras
if [ -f $BOARD_RC_EXTRAS ]
then
echo "Board extras: ${BOARD_RC_EXTRAS}"
. $BOARD_RC_EXTRAS
fi
unset BOARD_RC_EXTRAS
#
# Start any custom addons from the sdcard.
#
if [ -f $FEXTRAS ]
then
echo "Addons script: ${FEXTRAS}"
. $FEXTRAS
fi
#
# Start the logger.
#
. ${R}etc/init.d/rc.logging
#
# Set additional parameters and env variables for selected AUTOSTART.
#
if ! param compare SYS_AUTOSTART 0
then
. ${R}etc/init.d/rc.autostart.post
fi
if ! param compare SYS_PARAM_VER ${PARAM_DEFAULTS_VER}
then
echo "Switched to different parameter version. Resetting parameters."
param set SYS_PARAM_VER ${PARAM_DEFAULTS_VER}
param set SYS_AUTOCONFIG 2
param save
reboot
fi
#
# End of autostart.
#
fi
#
# Unset all script parameters to free RAM.
#
unset R
unset AUTOCNF
unset AUX_MODE
unset DATAMAN_OPT
unset FAILSAFE
unset FAILSAFE_AUX
unset FCONFIG
unset FEXTRAS
unset FMU_MODE
unset FRC
unset IO_PRESENT
unset IOFW
unset LOG_FILE
unset LOGGER_ARGS
unset LOGGER_BUF
unset MAV_TYPE
unset MIXER
unset MIXER_AUX
unset MIXER_FILE
unset MK_MODE
unset MKBLCTRL_ARG
unset OUTPUT_MODE
unset PARAM_DEFAULTS_VER
unset PARAM_FILE
unset PWM_AUX_DISARMED
unset PWM_AUX_MAX
unset PWM_AUX_MIN
unset PWM_AUX_OUT
unset PWM_AUX_RATE
unset PWM_DISARMED
unset PWM_MAX
unset PWM_MIN
unset PWM_OUT
unset PWM_RATE
unset RC_INPUT_ARGS
unset SDCARD_MIXERS_PATH
unset STARTUP_TUNE
unset USE_IO
unset VEHICLE_TYPE
#
# Boot is complete, inform MAVLink app(s) that the system is now fully up and running.
#
mavlink boot_complete
- 下图从网络上找的的对应启动的顺序一个图示，版本不太一样，但是大致差不多
- 总结：
从上述的图片分析，可以知道，一个启动脚本实现了挂载SD卡，设置存储好的一些飞行控制的初始参数（可以通过地面站修改），启动所有的外设传感器，启动与地面站通行的Mavlink服务，以及机型的选择后对应启动的控制服务（以固定翼为例，启动了ekf2，fw_att_control，fw_pos_control_l1，airspeed_selector，land_detector等服务），打开navigator服务。
1.2 src下在固定翼模式下modules中启动的app有哪些
我们可以通过地面站连接飞控，然后通过mavlink控制台，读取各个应用的工作状态，输入help可以看见一些可以使用的app, 然后在控制台输入app status可以查询其状态，通过查询得知以下src中的app被启动了：
-
airspeed_selector
-
battery_status
-
commander
-
dataman
-
计算单元ekf2
-
fw_att_control
-
fw_pos_control_l1
-
land_detector工作在固定翼模式下
-
load_mon
-
logger
-
mavlink
-
navigator
-
rc_update
-
sensors
-
uorb
二、PX4程序架构详解——固定翼
从上述分析我们知道了哪些app被启动了，接下来看一下，控制逻辑顺序，下图从网上找到的一个简化的模块使用的控制流程：
我们可以将上图从中间分成两个看，其中右边为传感器采集（GPS、optical_flow、inertial_sensors）回来的数据，并经过一些处理（position_estimator、attitude_estimator）发布到msg消息中，供左侧控制程序订阅使用。
px4的控制部分程序，采用的是分层设计。
-
用户设定commander和stickmapper生成的控制命令
-
控制命令使navigator去生成期望的位置信息
-
pos_ctrl通过期望位置与实际位置的偏差使用PID的算法输出期望姿态信息
-
att_ctrl通过期望姿态信息与实际姿态信息使用PID输出控制信息
-
经过混控器mixer根据机型进行力矩分配然后经过motor_driver输出实际舵面控制的pwm占空比
下图为各个模块之间的数据流：
2.1 commander
2.2 navigator
摘自PX4飞控之导航及任务架构
Navigator模块主要功能在于确定任务类型、地理围栏、失效保护，把任务航点更新后送给位置控制器。其中task_main函数是主函数。
在task_main 函数中，主要分为如下几个部分：
当commander中的传输来的导航模式为mission时，依次运行mission.cpp中的三个函数：初始化on_activation()、主函数on_active()、退出函数on_inactive()。
对于mission模式的主函数on_active()，代码逻辑如下：
mission的主要功能在于对航点数组pos_sp_triplet更新和赋值，航点数组包括previous、current、next三个航点，这一功能由函数set_mission_item()来实现，主要的逻辑为先从SD卡中读取航点信息read_mission_item()赋值给结构体mission_item，然后再将当前航点复制给pos_sp_triplet.current
每当一个航点任务完成is_mission_item_reached()返回值为true，则指针++，读取SD卡中的下一个航点。而判断航点的是否完成由三个判断条件：
航向控制由函数heading_sp_update()实现，将航向setpoint计算赋值给pos_sp_triplet.current.yaw,给到pos控制环，进行航向控制。
至此，导航完成了航点经纬高、航点类型、航向等信息的更新。而位置控制环以pos_sp_triplet为目标进行控制。
注：航点数组中的高度为绝对高度。
2.3 fw_pos_control_l1
2.3.1 程序框架
fw_pos_control_l1思维导图
2.3.2 总能量控制系统
摘自px4.io官网
1、Total Energy Control System (TECS)：总能量控制系统
该模块可以同时控制固定翼飞机的真实空速和高度
如上图所示，TECS接收空速和高度设定点作为输入，并输出油门和俯仰角设定点。 这两个输出被发送到固定翼姿态控制器，后者执行姿态控制解决方案。 因此，重要的是要了解，TECS的性能直接受变桨控制回路的性能影响。 对空速和高度的不良跟踪通常是由对飞机俯仰角的不良跟踪引起的。
同时控制真实空速和高度并不是一件容易的事。 飞机俯仰角的增加将导致高度的增加，但也会导致空速的降低。 增加节气门将增加空速，但由于升力的增加，高度也会增加。 因此，我们有两个输入（俯仰角和节气门），它们都影响两个输出（空速和高度），这使控制问题变得很困难。
TECS通过用能量而不是原始设定值来表示问题来提供解决方案。飞机的总能量是动能和势能之和。推力（通过节气门控制）增加了飞机的总能量状态。给定的总能量状态可以通过势能和动能的任意组合来实现。换句话说，在总的能量意义上，在高海拔但以低速飞行可以等同于在低海拔但以更快的空速飞行。我们将此称为特定能量平衡，它是根据当前高度和真实空速设定点计算得出的。通过飞机俯仰角控制比能量平衡。桨距角的增加将动能转移到势能，而桨距角为负的情况则相反。因此，通过将初始设定值转换为可以独立控制的能量来消除控制问题。我们使用推力来调节车辆的总能量，而俯仰角则在势能（高度）和动能（速度）之间保持特定的平衡。
2、总能量控制回路
3、总能量平衡控制回路
2.3.2 总能量控制系统实现（tecs_update_pitch_throttl();）
void
FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, float alt_sp, float airspeed_sp,
float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_cruise,
bool climbout_mode, float climbout_pitch_min_rad,
uint8_t mode)
{
const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, 0.01f, 0.05f);
_last_tecs_update = now;
// do not run TECS if we are not in air
bool run_tecs = !_vehicle_land_detected.landed;
// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
// (it should also not run during VTOL blending because airspeed is too low still)
if (_vehicle_status.is_vtol) {
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) {
run_tecs = false;
}
if (_vehicle_status.in_transition_mode) {
// we're in transition
_was_in_transition = true;
// set this to transition airspeed to init tecs correctly
if (_param_fw_arsp_mode.get() == 1 && PX4_ISFINITE(_param_airspeed_trans)) {
// some vtols fly without airspeed sensor
_asp_after_transition = _param_airspeed_trans;
} else {
_asp_after_transition = _airspeed;
}
_asp_after_transition = constrain(_asp_after_transition, _param_fw_airspd_min.get(), _param_fw_airspd_max.get());
} else if (_was_in_transition) {
// after transition we ramp up desired airspeed from the speed we had coming out of the transition
_asp_after_transition += dt * 2; // increase 2m/s
if (_asp_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
airspeed_sp = max(_asp_after_transition, _airspeed);
} else {
_was_in_transition = false;
_asp_after_transition = 0;
}
}
}
_is_tecs_running = run_tecs;
if (!run_tecs) {
// next time we run TECS we should reinitialize states
_reinitialize_tecs = true;
return;
}
if (_reinitialize_tecs) {
_tecs.reset_state();
_reinitialize_tecs = false;
}
if (_vehicle_status.engine_failure) {
/* Force the slow downwards spiral */
pitch_min_rad = M_DEG_TO_RAD_F * -1.0f;
pitch_max_rad = M_DEG_TO_RAD_F * 5.0f;
}
/* No underspeed protection in landing mode */
_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND
|| mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
/* Using tecs library */
float pitch_for_tecs = _pitch - radians(_param_fw_psp_off.get());
/* filter speed and altitude for controller */
Vector3f accel_body(_vehicle_acceleration_sub.get().xyz);
// tailsitters use the multicopter frame as reference, in fixed wing
// we need to use the fixed wing frame
if (_vtol_tailsitter) {
float tmp = accel_body(0);
accel_body(0) = -accel_body(2);
accel_body(2) = tmp;
}
/* tell TECS to update its state, but let it know when it cannot actually control the plane */
bool in_air_alt_control = (!_vehicle_land_detected.landed &&
(_control_mode.flag_control_auto_enabled ||
_control_mode.flag_control_offboard_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_altitude_enabled));
/* update TECS vehicle state estimates */
_tecs.update_vehicle_state_estimates(_airspeed, _R_nb,
accel_body, (_local_pos.timestamp > 0), in_air_alt_control,
_current_altitude, _local_pos.vz);
/* scale throttle cruise by baro pressure */
if (_param_fw_thr_alt_scl.get() > FLT_EPSILON) {
vehicle_air_data_s air_data;
if (_vehicle_air_data_sub.copy(&air_data)) {
if (PX4_ISFINITE(air_data.baro_pressure_pa) && PX4_ISFINITE(_param_fw_thr_alt_scl.get())) {
// scale throttle as a function of sqrt(p0/p) (~ EAS -> TAS at low speeds and altitudes ignoring temperature)
const float eas2tas = sqrtf(CONSTANTS_STD_PRESSURE_PA / air_data.baro_pressure_pa);
const float scale = constrain((eas2tas - 1.0f) * _param_fw_thr_alt_scl.get() + 1.f, 1.f, 2.f);
throttle_max = constrain(throttle_max * scale, throttle_min, 1.0f);
throttle_cruise = constrain(throttle_cruise * scale, throttle_min + 0.01f, throttle_max - 0.01f);
}
}
}
_tecs.update_pitch_throttle(_R_nb, pitch_for_tecs,
_current_altitude, alt_sp,
airspeed_sp, _airspeed, _eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
tecs_status_publish();
}
2.3.2总体位置控制逻辑 control_position()；
这里程序的流程图，可能有错误，还有待修正，仅供参考
bool
FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2f &curr_pos,
const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
{
const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);
//记录上次位置控制的时间
_control_position_last_called = now;
_l1_control.set_dt(dt);
/* only run position controller in fixed-wing mode and during transitions for VTOL */
/*仅在固定翼模式和VTOL过渡期间运行位置控制器*/
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) {
_control_mode_current = FW_POSCTRL_MODE_OTHER;
return false;
}
bool setpoint = true;
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder
//默认不使用方向舵去控制偏航
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; // by default we don't use flaps
//默认不打开襟翼
Vector2f nav_speed_2d{ground_speed};
if (_airspeed_valid) {
// l1 navigation logic breaks down when wind speed exceeds max airspeed
//当风速超过最大空速时，l1导航逻辑失效
// compute 2D groundspeed from airspeed-heading projection
//从空速航向投影计算2D地面速度
const Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
// angle between air_speed_2d and ground_speed
//air_speed_2d和ground_speed之间的角度（）
const float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));
// if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
//如果角度> 90度或地面速度小于阈值，则用空速投影代替地面速度
if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
nav_speed_2d = air_speed_2d;
}
}
/* no throttle limit as default */
/* 最大默认油门 */
float throttle_max = 1.0f;
/* save time when airplane is in air */
/*不在空中，也没有在地面，认为是滑跑刚起来，认为是在空中，节省时间*/
if (!_was_in_air && !_vehicle_land_detected.landed) {
_was_in_air = true;
_time_went_in_air = now;
_takeoff_ground_alt = _current_altitude;
}
/* reset flag when airplane landed */
/*飞机降落时重置标志*/
if (_vehicle_land_detected.landed) {
_was_in_air = false;
}
/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
/*如果从posctl处于非活动状态的其他模式切换到该模式，请重置积分器*/
if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
/* reset integrators */
_tecs.reset_state();
}
if ((_control_mode.flag_control_auto_enabled || _control_mode.flag_control_offboard_enabled) && pos_sp_curr.valid) {
/* AUTONOMOUS FLIGHT */
/* 自动飞行*/
_control_mode_current = FW_POSCTRL_MODE_AUTO;
/* reset hold altitude */
/*重置保持高度*/
_hold_alt = _current_altitude;
/* reset hold yaw */
/*重置偏航角*/
_hdg_hold_yaw = _yaw;
/* get circle mode */
/*获取是否是盘旋状态*/
bool was_circle_mode = _l1_control.circle_mode();
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
/*存储TECS参数*/
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
_tecs.set_time_const_throt(_param_fw_t_thro_const.get());
Vector2f curr_wp{0.0f, 0.0f};
Vector2f prev_wp{0.0f, 0.0f};
if (_vehicle_status.in_transition_to_fw) {
if (!PX4_ISFINITE(_transition_waypoint(0))) {
double lat_transition, lon_transition;
// create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track
// during the transition
//创建一个虚拟的航路点，L1控制器可以在过渡期间跟踪这个航路点
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition,
&lon_transition);
_transition_waypoint(0) = (float)lat_transition;
_transition_waypoint(1) = (float)lon_transition;
}
curr_wp = prev_wp = _transition_waypoint;
} else {
/* current waypoint (the one currently heading for) */
/*当前航路点*/
curr_wp = Vector2f((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
if (pos_sp_prev.valid) {
prev_wp(0) = (float)pos_sp_prev.lat;
prev_wp(1) = (float)pos_sp_prev.lon;
} else {
/*
* No valid previous waypoint, go for the current wp.
* This is automatically handled by the L1 library.
*/
//如果没有先前的航路点，使用当前的
prev_wp(0) = (float)pos_sp_curr.lat;
prev_wp(1) = (float)pos_sp_curr.lon;
}
/* reset transition waypoint, will be set upon entering front transition */
//重置航路点
_transition_waypoint.setNaN();
}
/* Initialize attitude controller integrator reset flags to 0 */
//初始姿态控制器的积分器复位到0
_att_sp.roll_reset_integral = false;
_att_sp.pitch_reset_integral = false;
_att_sp.yaw_reset_integral = false;
float mission_airspeed = _param_fw_airspd_trim.get();
if (PX4_ISFINITE(pos_sp_curr.cruising_speed) &&
pos_sp_curr.cruising_speed > 0.1f) {
mission_airspeed = pos_sp_curr.cruising_speed;
}
float mission_throttle = _param_fw_thr_cruise.get();
if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) &&
pos_sp_curr.cruising_throttle >= 0.0f) {
mission_throttle = pos_sp_curr.cruising_throttle;
}
const float acc_rad = _l1_control.switch_distance(500.0f);
uint8_t position_sp_type = pos_sp_curr.type;
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
// TAKEOFF: handle like a regular POSITION setpoint if already flying
//起飞点：如果已经飞起来就像普通航路点去处理
if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) {
// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
//起飞点变成普通点
position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
}
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
|| pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
float dist_xy = -1.f;
float dist_z = -1.f;
const float dist = get_distance_to_point_global_wgs84(
(double)curr_wp(0), (double)curr_wp(1), pos_sp_curr.alt,
_current_latitude, _current_longitude, _current_altitude,
&dist_xy, &dist_z);
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
// POSITION: achieve position setpoint altitude via loiter
//普通点：通过盘旋去达到普通点
// close to waypoint, but altitude error greater than twice acceptance
if ((dist >= 0.f)
&& (dist_z > 2.f * _param_fw_clmbout_diff.get())
&& (dist_xy < 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)))) {
// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
} else if ((dist_xy < 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) && !pos_sp_next.valid) {
position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
}
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
// LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER
if ((dist >= 0.f)
&& (dist_z > 2.f * _param_fw_clmbout_diff.get())
&& (dist_xy > 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)))) {
// SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION
position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
}
}
}
_type = position_sp_type;
if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.thrust_body[0] = 0.0f;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
// waypoint is a plain navigation waypoint
float position_sp_alt = pos_sp_curr.alt;
// Altitude first order hold (FOH)
if (pos_sp_prev.valid && PX4_ISFINITE(pos_sp_prev.alt) &&
((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) ||
(pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER) ||
(pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF))
) {
const float d_curr_prev = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1),
pos_sp_prev.lat, pos_sp_prev.lon);
// Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one
if (d_curr_prev > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) {
// Calculate distance to current waypoint
const float d_curr = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1),
_current_latitude, _current_longitude);
// Save distance to waypoint if it is the smallest ever achieved, however make sure that
// _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp
_min_current_sp_distance_xy = math::min(math::min(d_curr, _min_current_sp_distance_xy), d_curr_prev);
// if the minimal distance is smaller than the acceptance radius, we should be at waypoint alt
// navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude
if (_min_current_sp_distance_xy > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) {
// The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
// radius around the current waypoint
const float delta_alt = (pos_sp_curr.alt - pos_sp_prev.alt);
const float grad = -delta_alt / (d_curr_prev - math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)));
const float a = pos_sp_prev.alt - grad * d_curr_prev;
position_sp_alt = a + grad * _min_current_sp_distance_xy;
}
}
}
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
float tecs_fw_thr_min;
float tecs_fw_thr_max;
float tecs_fw_mission_throttle;
if (mission_throttle < _param_fw_thr_min.get()) {
/* enable gliding with this waypoint */
_tecs.set_speed_weight(2.0f);
tecs_fw_thr_min = 0.0;
tecs_fw_thr_max = 0.0;
tecs_fw_mission_throttle = 0.0;
} else {
tecs_fw_thr_min = _param_fw_thr_min.get();
tecs_fw_thr_max = _param_fw_thr_max.get();
tecs_fw_mission_throttle = mission_throttle;
}
tecs_update_pitch_throttle(now, position_sp_alt,
calculate_target_airspeed(mission_airspeed, ground_speed),
radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()),
radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()),
tecs_fw_thr_min,
tecs_fw_thr_max,
tecs_fw_mission_throttle,
false,
radians(_param_fw_p_lim_min.get()));
} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
/* waypoint is a loiter waypoint */
float loiter_radius = pos_sp_curr.loiter_radius;
uint8_t loiter_direction = pos_sp_curr.loiter_direction;
if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) {
loiter_radius = _param_nav_loiter_rad.get();
loiter_direction = (loiter_radius > 0) ? 1 : -1;
}
_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, nav_speed_2d);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
float alt_sp = pos_sp_curr.alt;
if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid
&& _l1_control.circle_mode() && _param_fw_lnd_earlycfg.get()) {
// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
// landing airspeed and potentially tighter throttle control) already such that we don't
// have to do this switch (which can cause significant altitude errors) close to the ground.
_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
mission_airspeed = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
_att_sp.apply_flaps = true;
}
if (in_takeoff_situation()) {
alt_sp = max(alt_sp, _takeoff_ground_alt + _param_fw_clmbout_diff.get());
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-5.0f), radians(5.0f));
}
if (_land_abort) {
if (pos_sp_curr.alt - _current_altitude < _param_fw_clmbout_diff.get()) {
// aborted landing complete, normal loiter over landing point
abort_landing(false);
} else {
// continue straight until vehicle has sufficient altitude
_att_sp.roll_body = 0.0f;
}
_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
}
float tecs_fw_thr_min;
float tecs_fw_thr_max;
float tecs_fw_mission_throttle;
if (mission_throttle < _param_fw_thr_min.get()) {
/* enable gliding with this waypoint */
_tecs.set_speed_weight(2.0f);
tecs_fw_thr_min = 0.0;
tecs_fw_thr_max = 0.0;
tecs_fw_mission_throttle = 0.0;
} else {
tecs_fw_thr_min = _param_fw_thr_min.get();
tecs_fw_thr_max = _param_fw_thr_max.get();
tecs_fw_mission_throttle = _param_fw_thr_cruise.get();
}
tecs_update_pitch_throttle(now, alt_sp,
calculate_target_airspeed(mission_airspeed, ground_speed),
radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()),
radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()),
tecs_fw_thr_min,
tecs_fw_thr_max,
tecs_fw_mission_throttle,
false,
radians(_param_fw_p_lim_min.get()));
} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LAND) {
control_landing(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
control_takeoff(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
}
/* reset landing state */
if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) {
reset_landing_state();
}
/* reset takeoff/launch state */
if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
reset_takeoff_state();
}
if (was_circle_mode && !_l1_control.circle_mode()) {
/* just kicked out of loiter, reset roll integrals */
_att_sp.roll_reset_integral = true;
}
} else if (_control_mode.flag_control_velocity_enabled &&
_control_mode.flag_control_altitude_enabled) {
/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed,
heading is set to a distant waypoint */
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _current_altitude;
_hdg_hold_yaw = _yaw;
_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
_yaw_lock_engaged = false;
/* reset setpoints from other modes (auto) otherwise we won't
* level out without new manual input */
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.yaw_body = 0;
}
_control_mode_current = FW_POSCTRL_MODE_POSITION;
float altctrl_airspeed = get_demanded_airspeed();
/* update desired altitude based on user pitch stick input */
bool climbout_requested = update_desired_altitude(dt);
// if we assume that user is taking off then help by demanding altitude setpoint well above ground
// and set limit to pitch angle to prevent steering into ground
// this will only affect planes and not VTOL
float pitch_limit_min = _param_fw_p_lim_min.get();
do_takeoff_help(&_hold_alt, &pitch_limit_min);
/* throttle limiting */
throttle_max = _param_fw_thr_max.get();
if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint.z) < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
tecs_update_pitch_throttle(now, _hold_alt,
altctrl_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
throttle_max,
_param_fw_thr_cruise.get(),
climbout_requested,
climbout_requested ? radians(10.0f) : pitch_limit_min,
tecs_status_s::TECS_MODE_NORMAL);
/* heading control */
if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */
if (fabsf(_vehicle_rates_sub.get().xyz[2]) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
// little yaw movement, lock to current heading
_yaw_lock_engaged = true;
}
/* user tries to do a takeoff in heading hold mode, reset the yaw setpoint on every iteration
to make sure the plane does not start rolling
*/
if (in_takeoff_situation()) {
_hdg_hold_enabled = false;
_yaw_lock_engaged = true;
}
if (_yaw_lock_engaged) {
/* just switched back from non heading-hold to heading hold */
if (!_hdg_hold_enabled) {
_hdg_hold_enabled = true;
_hdg_hold_yaw = _yaw;
get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
}
/* we have a valid heading hold position, are we too close? */
float dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _hdg_hold_curr_wp.lat,
_hdg_hold_curr_wp.lon);
if (dist < HDG_HOLD_REACHED_DIST) {
get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false);
}
Vector2f prev_wp{(float)_hdg_hold_prev_wp.lat, (float)_hdg_hold_prev_wp.lon};
Vector2f curr_wp{(float)_hdg_hold_curr_wp.lat, (float)_hdg_hold_curr_wp.lon};
/* populate l1 control setpoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
if (in_takeoff_situation()) {
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));
}
}
}
if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
fabsf(_manual_control_setpoint.r) >= HDG_HOLD_MAN_INPUT_THRESH) {
_hdg_hold_enabled = false;
_yaw_lock_engaged = false;
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.yaw_body = 0;
}
} else if (_control_mode.flag_control_altitude_enabled) {
/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _current_altitude;
}
_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
/* Get demanded airspeed */
float altctrl_airspeed = get_demanded_airspeed();
/* update desired altitude based on user pitch stick input */
bool climbout_requested = update_desired_altitude(dt);
// if we assume that user is taking off then help by demanding altitude setpoint well above ground
// and set limit to pitch angle to prevent steering into ground
// this will only affect planes and not VTOL
float pitch_limit_min = _param_fw_p_lim_min.get();
do_takeoff_help(&_hold_alt, &pitch_limit_min);
/* throttle limiting */
throttle_max = _param_fw_thr_max.get();
if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint.z) < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
tecs_update_pitch_throttle(now, _hold_alt,
altctrl_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
throttle_max,
_param_fw_thr_cruise.get(),
climbout_requested,
climbout_requested ? radians(10.0f) : pitch_limit_min,
tecs_status_s::TECS_MODE_NORMAL);
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.yaw_body = 0;
} else {
_control_mode_current = FW_POSCTRL_MODE_OTHER;
/* do not publish the setpoint */
setpoint = false;
// reset hold altitude
_hold_alt = _current_altitude;
/* reset landing and takeoff state */
if (!_last_manual) {
reset_landing_state();
reset_takeoff_state();
}
}
/* Copy thrust output for publication */
if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS &&
!_runway_takeoff.runwayTakeoffEnabled()) {
/* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons.
the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
_runway_takeoff.runwayTakeoffEnabled()) {
_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, min(get_tecs_thrust(), throttle_max));
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.thrust_body[0] = 0.0f;
} else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());
} else {
/* Copy thrust and pitch values from tecs */
if (_vehicle_land_detected.landed) {
// when we are landed state we want the motor to spin at idle speed
_att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), throttle_max);
} else {
_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
}
}
// decide when to use pitch setpoint from TECS because in some cases pitch
// setpoint is generated by other means
bool use_tecs_pitch = true;
// auto runway takeoff
use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
(_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled()));
// flaring during landing
use_tecs_pitch &= !(pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _land_noreturn_vertical);
// manual attitude control
use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_OTHER);
if (use_tecs_pitch) {
_att_sp.pitch_body = get_tecs_pitch();
}
if (_control_mode.flag_control_position_enabled) {
_last_manual = false;
} else {
_last_manual = true;
}
return setpoint;
}
2.4 fw_att_control
2.4.1程序框架
fw_att_control的思维导图
2.4.2 姿态控制器
姿态控制器使用级联回路方法工作。 外环计算姿态设定值和估算姿态之间的误差，该误差乘以增益（P控制器）即可生成速率设定点。 然后，内部环路计算出速率误差，并使用PI（比例+积分）控制器生成所需的角加速度。
然后，使用此期望的角加速度和通过控制分配（也称为混合）对系统的先验知识，计算控制执行器（飞机，升降舵，舵等）的角位置。 此外，由于控制面在高速时更有效，而在低速时则较差，因此使用空速测量（如果使用此类传感器）来缩放针对巡航速度进行调整的控制器。
前馈增益FF用于补偿空气动力学阻尼。 基本上，飞机上机轴力矩的两个主要组成部分是由控制面（飞机，升降舵，方向舵，产生运动）和空气动力学阻尼（与机率成比例，抵消运动）产生的。 为了保持恒定的速率，可以在速率环路中使用前馈来补偿该阻尼。
侧倾和俯仰控制器具有相同的结构，并且假定纵向和横向动力学已解耦到足以独立工作。 但是，偏航控制器使用转角协调约束来生成其偏航率设定值，以使飞机滑行时产生的横向加速度最小。 偏航率控制器还有助于抵消不利的偏航影响，并通过提供额外的定向阻尼来抑制荷兰滚
2.4.3 姿态控制器程序实现（ecl_pitch_controller.cpp、ecl_roll_controller.cpp、ecl_yaw_controller.cpp）
1、外环计算姿态设定值和估算姿态之间的误差，该误差乘以增益（P控制器）即可生成速率设定点
float ECL_PitchController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.airspeed))) {
return _rate_setpoint;
}
/* Calculate the error */
float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
return _rate_setpoint;
}
2、将欧垃角速率转换到机体角速率
float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(dt, ctl_data);
}
3、内部环路计算出速率误差，并使用PI（比例+积分）控制器生成所需的角加速度。
float ECL_PitchController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.body_y_rate) &&
PX4_ISFINITE(ctl_data.body_z_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_y_rate;
if (!ctl_data.lock_integrator && _k_i > 0.0f) {
/* Integral term scales with 1/IAS^2 */
float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
/* add and constrain */
_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
}
/* Apply PI rate controller and store non-limited output */
/* FF terms scales with 1/TAS and P,I with 1/IAS^2 */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ _integrator;
return math::constrain(_last_output, -1.0f, 1.0f);
}
4、最终输出_last_output送入到混控器当中
2.4.3 总体姿态控制逻辑(FixedwingAttitudeControl.cpp)
[流程图]还未做完
void FixedwingAttitudeControl::Run()
{
if (should_exit()) {
_att_sub.unregisterCallback();
exit_and_cleanup();
return;
}
perf_begin(_loop_perf);
// only run controller if attitude changed
vehicle_attitude_s att;
if (_att_sub.update(&att)) {
// 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();
}
const float dt = math::constrain((att.timestamp - _last_run) * 1e-6f, 0.002f, 0.04f);
_last_run = att.timestamp;
/* get current rotation matrix and euler angles from control state quaternions */
matrix::Dcmf R = matrix::Quatf(att.q);
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];
if (_is_tailsitter) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
* Since the VTOL airframe is initialized as a multicopter we need to
* modify the estimated attitude for the fixed wing operation.
* Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
* the pitch axis compared to the neutral position of the vehicle in multicopter mode
* we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
* Additionally, in order to get the correct sign of the pitch, we need to multiply
* the new x axis of the rotation matrix with -1
*
* original: modified:
*
* Rxx Ryx Rzx -Rzx Ryx Rxx
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
matrix::Dcmf R_adapted = R; //modified rotation matrix
/* move z to x */
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
/* move x to z */
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
/* fill in new attitude data */
R = R_adapted;
/* lastly, roll- and yawspeed have to be swaped */
float helper = rollspeed;
rollspeed = -yawspeed;
yawspeed = helper;
}
const matrix::Eulerf euler_angles(R);
vehicle_attitude_setpoint_poll();
// vehicle status update must be before the vehicle_control_mode_poll(), otherwise rate sp are not published during whole transition
_vehicle_status_sub.update(&_vehicle_status);
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_land_detected_poll();
// the position controller will not emit attitude setpoints in some modes
// we need to make sure that this flag is reset
_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;
bool wheel_control = false;
// TODO: manual wheel_control on ground?
if (_param_fw_w_en.get() && _att_sp.fw_control_yaw) {
wheel_control = true;
}
/* lock integrator until control is started or for long intervals (> 20 ms) */
bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled
|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && ! _vehicle_status.in_transition_mode)
|| (dt > 0.02f);
/* if we are in rotary wing mode, do nothing */
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) {
perf_end(_loop_perf);
return;
}
control_flaps(dt);
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_rates_enabled) {
const float airspeed = get_airspeed_and_update_scaling();
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
/* Reset integrators if the aircraft is on ground
* or a multicopter (but not transitioning VTOL)
*/
if (_landed
|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& !_vehicle_status.in_transition_mode)) {
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
/* Prepare data for attitude controllers */
ECL_ControlData control_input{};
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.airspeed_min = _param_fw_airspd_min.get();
control_input.airspeed_max = _param_fw_airspd_max.get();
control_input.airspeed = airspeed;
control_input.scaler = _airspeed_scaling;
control_input.lock_integrator = lock_integrator;
if (wheel_control) {
_local_pos_sub.update(&_local_pos);
/* Use min 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_min.get() * 0.6f);
float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed);
control_input.groundspeed = groundspeed;
control_input.groundspeed_scaler = groundspeed_scaler;
}
/* 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 += gradual(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_r_vmin.get(),
0.0f);
trim_pitch += gradual(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_p_vmin.get(),
0.0f);
trim_yaw += gradual(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_y_vmin.get(),
0.0f);
} else {
trim_roll += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_param_fw_dtrim_r_vmax.get());
trim_pitch += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_param_fw_dtrim_p_vmax.get());
trim_yaw += gradual(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_applied * _param_fw_dtrim_r_flps.get();
trim_pitch += _flaps_applied * _param_fw_dtrim_p_flps.get();
/* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled) {
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);
if (wheel_control) {
_wheel_ctrl.control_attitude(dt, control_input);
_yaw_ctrl.reset_integrator();
} else {
// runs last, because is depending on output of roll and pitch attitude
_yaw_ctrl.control_attitude(dt, control_input);
_wheel_ctrl.reset_integrator();
}
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_euler_rate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
}
float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
}
float yaw_u = 0.0f;
if (wheel_control) {
yaw_u = _wheel_ctrl.control_bodyrate(dt, control_input);
} else {
yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input);
}
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
/* add in manual rudder control in manual modes */
if (_vcontrol_mode.flag_control_manual_enabled) {
_actuators.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r;
}
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
/* throttle passed through if it is finite and if no engine failure was detected */
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0])
&& !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f;
/* scale effort by battery status */
if (_param_fw_bat_scale_en.get() &&
_actuators.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)) {
if (battery_status.scale > 0.0f) {
_battery_scale = battery_status.scale;
}
}
}
_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
}
}
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
_rates_sp.roll = _roll_ctrl.get_desired_bodyrate();
_rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate();
_rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate();
_rates_sp.timestamp = hrt_absolute_time();
_rate_sp_pub.publish(_rates_sp);
} else {
vehicle_rates_setpoint_poll();
_roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll);
_yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw);
_pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch);
float roll_u = _roll_ctrl.control_bodyrate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
float pitch_u = _pitch_ctrl.control_bodyrate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
float yaw_u = _yaw_ctrl.control_bodyrate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ?
_rates_sp.thrust_body[0] : 0.0f;
}
rate_ctrl_status_s rate_ctrl_status{};
rate_ctrl_status.timestamp = hrt_absolute_time();
rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator();
rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator();
if (wheel_control) {
rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator();
} else {
rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator();
}
_rate_ctrl_status_pub.publish(rate_ctrl_status);
}
// 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
_actuators.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get()
* constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;
_actuators.control[5] = _manual_control_setpoint.aux1;
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
_actuators.control[7] = _manual_control_setpoint.aux3;
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.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) {
_actuators_0_pub.publish(_actuators);
}
}
perf_end(_loop_perf);
}
void FixedwingAttitudeControl::control_flaps(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
&& fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
flap_control = 0.5f * (_manual_control_setpoint.flaps + 1.0f) * _param_fw_flaps_scl.get();
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
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 = 1.0f * _param_fw_flaps_scl.get() * _param_fw_flaps_lnd_scl.get();
break;
case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF:
flap_control = 1.0f * _param_fw_flaps_scl.get() * _param_fw_flaps_to_scl.get();
break;
}
}
// move the actual control value continuous with time, full flap travel in 1sec
if (fabsf(_flaps_applied - flap_control) > 0.01f) {
_flaps_applied += (_flaps_applied - flap_control) < 0 ? dt : -dt;
} else {
_flaps_applied = flap_control;
}
/* default flaperon to center */
float flaperon_control = 0.0f;
/* map flaperons by default to manual if valid */
if (PX4_ISFINITE(_manual_control_setpoint.aux2) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
flaperon_control = 0.5f * (_manual_control_setpoint.aux2 + 1.0f) * _param_fw_flaperon_scl.get();
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
if (_att_sp.apply_flaps == vehicle_attitude_setpoint_s::FLAPS_LAND) {
flaperon_control = _param_fw_flaperon_scl.get();
} else {
flaperon_control = 0.0f;
}
}
// move the actual control value continuous with time, full flap travel in 1sec
if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) {
_flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? dt : -dt;
} else {
_flaperons_applied = flaperon_control;
}
}