PX4程序架构
一、从RCS启动脚本可以看出哪些东西
启动脚本是一个神奇的东西,它能够识别出你对应的飞机类型,加载对应的混控器,选择对应的姿态、位置估计程序以及控制程序,初始化你需要的驱动程序。下面来分析下。
1.1RCS位置
- Firmware->ROMFS->px4fmu_common->init.d->rcs,最新版本的启动脚本如下:
#!/bin/sh
set +e
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
set PARAM_DEFAULTS_VER 1
mount -t procfs /proc
sercon
ver all
uorb start
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
if hardfault_log check
then
set STARTUP_TUNE 2
if hardfault_log commit
then
hardfault_log reset
fi
fi
if [ ! -f "/fs/microsd/.metadata_never_index" ]; then
cat > /fs/microsd/.metadata_never_index
fi
if [ ! -f "/fs/microsd/.Trashes" ]; then
cat > /fs/microsd/.Trashes
fi
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
if [ ! -f "/fs/microsd/.Trash-1000" ]; then
cat > /fs/microsd/.Trash-1000
fi
else
set STARTUP_TUNE 14
if mkfatfs /dev/mmcsd0
then
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
echo "INFO [init] card formatted"
else
set STARTUP_TUNE 15
echo "ERROR [init] format failed"
set LOG_FILE /dev/null
fi
else
set LOG_FILE /dev/null
fi
fi
if [ -f $FRC ]
then
. $FRC
else
if mtd start
then
set PARAM_FILE /fs/mtd_params
fi
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
if param greater SYS_AUTOCONFIG 0
then
if param compare SYS_AUTOCONFIG 1
then
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
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
tone_alarm start
param compare CBRK_BUZZER 782090
if [ $? != 0 -o $STARTUP_TUNE != 1 ]
then
tune_control play -t $STARTUP_TUNE
fi
dataman start $DATAMAN_OPT
send_event start
load_mon start
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
if ! param compare SYS_AUTOSTART 0
then
. ${R}etc/init.d/rc.autostart
fi
if [ -f $FCONFIG ]
then
echo "Custom: ${FCONFIG}"
. $FCONFIG
fi
if [ $AUTOCNF = yes ]
then
param set SYS_AUTOCONFIG 0
fi
if [ -f $IOFW ]
then
if ver hwtypecmp V540 V560
then
param set SYS_USE_IO 0
else
if px4io checkcrc ${IOFW}
then
set IO_PRESENT yes
else
tune_control play -t 16
if px4io start
then
if ! px4io safety_on
then
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
set IO_PRESENT yes
fi
fi
if [ $IO_PRESENT = no ]
then
echo "PX4IO update failed" >> $LOG_FILE
tune_control play -t 18
fi
fi
fi
fi
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 start
if param greater SYS_HITL 0
then
set OUTPUT_MODE hil
sensors start -h
commander start -h
param set GPS_1_CONFIG 0
if param compare SYS_HITL 2
then
sih start
fi
else
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
if param compare -s SENS_EN_LL40LS 1
then
set FMU_MODE pwm4
set AUX_MODE pwm4
fi
if param compare FD_EXT_ATS_EN 1
then
set FMU_MODE pwm4
set AUX_MODE pwm4
fi
if param greater TRIG_MODE 0
then
if param compare TRIG_PINS 56
then
set FMU_MODE pwm4
set AUX_MODE pwm4
else
if param compare TRIG_PINS 78
then
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
if param greater -s UAVCAN_ENABLE 0
then
if uavcan start
then
if param greater UAVCAN_ENABLE 1
then
uavcan start fw
if param greater UAVCAN_ENABLE 2
then
set OUTPUT_MODE uavcan_esc
fi
fi
else
tune_control play error
fi
fi
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
. ${R}etc/init.d/rc.serial
if [ $IO_PRESENT = no ]
then
rc_input start $RC_INPUT_ARGS
fi
. ${R}etc/init.d/rc.vehicle_setup
if param greater -s CAM_CAP_FBACK 0
then
if camera_capture start
then
camera_capture on
fi
fi
navigator start
. ${R}etc/init.d/rc.thermal_cal
if ! param compare MNT_MODE_IN -1
then
vmount start
fi
if param compare SENS_EN_PX4FLOW 1
then
px4flow start -X
fi
if param greater TEL_BST_EN 0
then
bst start -X
fi
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
if [ -f $FEXTRAS ]
then
echo "Addons script: ${FEXTRAS}"
. $FEXTRAS
fi
. ${R}etc/init.d/rc.logging
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
fi
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
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;
bool run_tecs = !_vehicle_land_detected.landed;
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) {
_was_in_transition = true;
if (_param_fw_arsp_mode.get() == 1 && PX4_ISFINITE(_param_airspeed_trans)) {
_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) {
_asp_after_transition += dt * 2;
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) {
_reinitialize_tecs = true;
return;
}
if (_reinitialize_tecs) {
_tecs.reset_state();
_reinitialize_tecs = false;
}
if (_vehicle_status.engine_failure) {
pitch_min_rad = M_DEG_TO_RAD_F * -1.0f;
pitch_max_rad = M_DEG_TO_RAD_F * 5.0f;
}
_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND
|| mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
float pitch_for_tecs = _pitch - radians(_param_fw_psp_off.get());
Vector3f accel_body(_vehicle_acceleration_sub.get().xyz);
if (_vtol_tailsitter) {
float tmp = accel_body(0);
accel_body(0) = -accel_body(2);
accel_body(2) = tmp;
}
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));
_tecs.update_vehicle_state_estimates(_airspeed, _R_nb,
accel_body, (_local_pos.timestamp > 0), in_air_alt_control,
_current_altitude, _local_pos.vz);
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())) {
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);
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;
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
Vector2f nav_speed_2d{ground_speed};
if (_airspeed_valid) {
const Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
const float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));
if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
nav_speed_2d = air_speed_2d;
}
}
float throttle_max = 1.0f;
if (!_was_in_air && !_vehicle_land_detected.landed) {
_was_in_air = true;
_time_went_in_air = now;
_takeoff_ground_alt = _current_altitude;
}
if (_vehicle_land_detected.landed) {
_was_in_air = false;
}
if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
_tecs.reset_state();
}
if ((_control_mode.flag_control_auto_enabled || _control_mode.flag_control_offboard_enabled) && pos_sp_curr.valid) {
_control_mode_current = FW_POSCTRL_MODE_AUTO;
_hold_alt = _current_altitude;
_hdg_hold_yaw = _yaw;
bool was_circle_mode = _l1_control.circle_mode();
_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;
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 {
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 {
prev_wp(0) = (float)pos_sp_curr.lat;
prev_wp(1) = (float)pos_sp_curr.lon;
}
_transition_waypoint.setNaN();
}
_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) {
if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) {
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) {
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)))) {
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) {
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)))) {
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) {
float position_sp_alt = pos_sp_curr.alt;
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);
if (d_curr_prev > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) {
const float d_curr = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1),
_current_latitude, _current_longitude);
_min_current_sp_distance_xy = math::min(math::min(d_curr, _min_current_sp_distance_xy), d_curr_prev);
if (_min_current_sp_distance_xy > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) {
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()) {
_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) {
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()) {
_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()) {
abort_landing(false);
} else {
_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()) {
_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);
}
if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) {
reset_landing_state();
}
if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
reset_takeoff_state();
}
if (was_circle_mode && !_l1_control.circle_mode()) {
_att_sp.roll_reset_integral = true;
}
} else if (_control_mode.flag_control_velocity_enabled &&
_control_mode.flag_control_altitude_enabled) {
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
_hold_alt = _current_altitude;
_hdg_hold_yaw = _yaw;
_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;
}
_control_mode_current = FW_POSCTRL_MODE_POSITION;
float altctrl_airspeed = get_demanded_airspeed();
bool climbout_requested = update_desired_altitude(dt);
float pitch_limit_min = _param_fw_p_lim_min.get();
do_takeoff_help(&_hold_alt, &pitch_limit_min);
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);
if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) {
if (fabsf(_vehicle_rates_sub.get().xyz[2]) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
_yaw_lock_engaged = true;
}
if (in_takeoff_situation()) {
_hdg_hold_enabled = false;
_yaw_lock_engaged = true;
}
if (_yaw_lock_engaged) {
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);
}
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};
_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()) {
_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) {
if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) {
_hold_alt = _current_altitude;
}
_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
float altctrl_airspeed = get_demanded_airspeed();
bool climbout_requested = update_desired_altitude(dt);
float pitch_limit_min = _param_fw_p_lim_min.get();
do_takeoff_help(&_hold_alt, &pitch_limit_min);
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;
setpoint = false;
_hold_alt = _current_altitude;
if (!_last_manual) {
reset_landing_state();
reset_takeoff_state();
}
}
if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS &&
!_runway_takeoff.runwayTakeoffEnabled()) {
_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 {
if (_vehicle_land_detected.landed) {
_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);
}
}
bool use_tecs_pitch = true;
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()));
use_tecs_pitch &= !(pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _land_noreturn_vertical);
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)
{
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;
}
float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
_rate_setpoint = pitch_error / _tc;
return _rate_setpoint;
}
2、将欧垃角速率转换到机体角速率
float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
{
_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)
{
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);
}
_rate_error = _bodyrate_setpoint - ctl_data.body_y_rate;
if (!ctl_data.lock_integrator && _k_i > 0.0f) {
float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
if (_last_output < -1.0f) {
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
id = math::min(id, 0.0f);
}
_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
}
_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);
vehicle_attitude_s att;
if (_att_sub.update(&att)) {
bool params_updated = _parameter_update_sub.updated();
if (params_updated) {
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
updateParams();
parameters_update();
}
const float dt = math::constrain((att.timestamp - _last_run) * 1e-6f, 0.002f, 0.04f);
_last_run = att.timestamp;
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) {
matrix::Dcmf R_adapted = R;
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
R = R_adapted;
float helper = rollspeed;
rollspeed = -yawspeed;
yawspeed = helper;
}
const matrix::Eulerf euler_angles(R);
vehicle_attitude_setpoint_poll();
_vehicle_status_sub.update(&_vehicle_status);
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_land_detected_poll();
_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;
bool wheel_control = false;
if (_param_fw_w_en.get() && _att_sp.fw_control_yaw) {
wheel_control = true;
}
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 (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) {
perf_end(_loop_perf);
return;
}
control_flaps(dt);
if (_vcontrol_mode.flag_control_rates_enabled) {
const float airspeed = get_airspeed_and_update_scaling();
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();
}
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();
}
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);
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;
}
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;
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());
}
trim_roll += _flaps_applied * _param_fw_dtrim_r_flps.get();
trim_pitch += _flaps_applied * _param_fw_dtrim_p_flps.get();
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 {
_yaw_ctrl.control_attitude(dt, control_input);
_wheel_ctrl.reset_integrator();
}
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();
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;
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();
}
_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;
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;
}
}
_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);
}
_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;
_actuators.control[7] = _manual_control_setpoint.aux3;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = att.timestamp;
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)
{
float flap_control = 0.0f;
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;
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;
}
}
if (fabsf(_flaps_applied - flap_control) > 0.01f) {
_flaps_applied += (_flaps_applied - flap_control) < 0 ? dt : -dt;
} else {
_flaps_applied = flap_control;
}
float flaperon_control = 0.0f;
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;
}
}
if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) {
_flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? dt : -dt;
} else {
_flaperons_applied = flaperon_control;
}
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)