PX4二次开发——PX4程序架构

最新推荐文章于 2024-03-25 10:31:38 发布
最新推荐文章于 2024-03-25 10:31:38 发布
阅读量3.3k 收藏 46
点赞数 6
分类专栏： PX4源码学习 文章标签： 操作系统 c++
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_36394290/article/details/109278011
版权

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、总能量控制回路

​ [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-9GQRHRyb-1603627599930)(https://dev.px4.io/master/assets/diagrams/TECS_throttle.jpg)]

​ 3、总能量平衡控制回路

​ [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-FTbk0jMB-1603627599931)(https://dev.px4.io/master/assets/diagrams/TECS_pitch.jpg)]

在这里插入图片描述

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 姿态控制器

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-IoaUm83f-1603627599933)(https://dev.px4.io/master/assets/diagrams/px4_fw_attitude_controller_diagram.png)]

​ 姿态控制器使用级联回路方法工作。 外环计算姿态设定值和估算姿态之间的误差，该误差乘以增益（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;
	}
}
多谱hhhh
关注
  • 6
    点赞
  • 踩
  • 46
    收藏
    觉得还不错? 一键收藏
  • 6
    评论
专栏目录
PX4架构详细介绍
02-02
详细的Pixhawk4源码架构介绍，里面以截图的形式，包含有根目录下二次开发的文件说明！
PX4二次开发——程序运行过程
qq_36394290的博客
10-16 4485
PX4二次开发——程序运行过程 一、写在前面 ​ px4固件程序与最开始我们所学习的对单片机外设开发不同，是因为飞行器控制系统是一个复杂的系统，要求实时性好，完成复杂的控制任务，简简单单的按照之前所学习简单单片机编程是不能满足要求的。 ​ 因为要满足实时处理各种数据，实现复杂的控制任务，这就要求PX4固件是搭载到操作系统上，对于STM32这款单片机，开源的PX4是基于Nuttx系统进行开发。所以，在我们阅读其程序流程的时候，不太容易找到程序运行的路线是如何，于是写下本文捋清思路。 ​ 首先，我觉得可以把整
开源飞控之王PX4
最新发布
u013558123的博客
03-25 583
PX4作为开源飞控的代表，凭借其卓越的性能和广泛的应用场景，赢得了众多无人机爱好者和开发者的青睐。相信在未来的发展中，PX4将继续引领无人机飞行控制技术的前沿，为无人机应用带来更多的可能性。
【PX4(一)】PX4二次开发环境搭建-QGroundcontrol配置和gazebo环境搭建
weixin_43728093的博客
10-24 2716
以前装PX4二次开发环境遇到了很多坑，也查过了很多帖子都很难达到理想的效果，现在重新捡起来，在安装时遇到的问题一个一个记录与解决，现在我把我成功安装与编译运行所遇到的问题与解决方案全都记录下来，希望给初入坑的人提供帮助。 linux系统：ubuntu18.04 (非虚拟机) 网络条件：可以上谷歌 一、下载源码和子模块 git clone -b v1.11.3 https://github.com/PX4/PX4-Autopilot.git 下载完毕后，界面如下： 进入源码目录： cd
[云讷科技] Kerloud PX4飞控的EKF2程序导航
云讷科技
01-07 3068
一、介绍 EKF拓展卡尔曼滤波器是px4开源飞控框架采用的核心状态估计方法，EKF2是px4飞控中的对应的软件模块，可以支持各类传感器信号，包括IMU，磁感计，激光测距仪，气压计，激光定位和视觉定位的数据等。 本文以Kerloud飞控支持的px4软件版本为例，系统地介绍下EKF的程序流程，Kerloud飞控是云讷科技的核心产品之一。 Kerloud飞控支持维护的px4软件库位于：https://github.com/cloudkernel-tech/Firmware Kerloud飞控的详细说明位于：htt
px4的模式与状态
OLillian的专栏
04-26 9706
一、模式 PX4 native flight stack String Description and notes MANUAL ACRO ALTCTL POSCTL OF
PX4 的ECL EKF2方程推导.pdf
05-18
无人机开源飞控px4中 关于EKF2部分的详细推导！
PX4从放弃到精通（十五）：二次开发基础
超维空间科技的博客
01-20 6653
文章目录前言一、自定义UORB消息二、创建自定义进程1.引入库2.读入数据总结 前言 一个人可以走的更快，一群人才能走的更远，交流学习加qq:2096723956 更多保姆级PX4+ROS学习视频：https://b23.tv/ZeUDKqy 分享知识，传递正能量，如有疏漏或不当之处，恳请指出． 固件版本：PX4 1.11.3 本文主要介绍利用PX4进行二次开发所需的基础知识，磨刀不误砍柴工，掌握这些知识是进行二次开发的前提． 一、自定义UORB消息 1.创建msg文件 PX4中所有的UORB消息都有一个m
PX4从放弃到精通（二十五）：EKF2
超维空间科技的博客
05-25 2200
PX4 ekf的传感器数据放在FIFO的环形缓冲区中，每次融合时取最新的数据，融合出的bias直接给输出滤波器，补偿到IMU的数据中，同时EKF融合出的位置速度等数据也要给输出滤波器，根据延时经过补偿输出当前时间线下的状态。
PX4 -- EKF2
lib0000的博客
04-25 1670
文章目录EKF2参数高度估计Range Finder滤波单变量更新单变量更新对多变量的影响 EKF2 参数 EKF2 中有一类 GATE 参数。当测量值在 VAR ± GATE 范围内才会更新值。 高度估计 四种高度控制方法：气压计，GPS，Range Finder，Vision。如果将 Range Finder 作为 Secondary Source （具体参考 Range Finder 篇），Primary Source 会在合适条件（一定高度，一定的速度下）切换为 Secondary Source 估
PX4-做飞控二次开发需要知道的事情-Cxm
chen_taifu的博客
08-06 2766
PX4飞控开发需要了解的事情
PX4-Pixhawk程序研究笔记
01-12
PX4-Pixhawk程序研究笔记
PixHawk开发PX4drivers.rar
07-21
PX4官方文档中PX4 USB Drivers，原链接已失效，现在传上来跟大家共享一下。
PX4源码解读——传感器校正（陀螺仪）.pdf
10-17
该文档从陀螺仪的数学模型，校正流程，px4源码解读三个方面讲解了px4传感器校正代码
PX4固件源码，亲测可编译
07-25
PX4固件源码，亲测可编译
搭建PX4开发环境
qq_36394290的博客
10-15 5572
搭建PX4开发环境 官方网站PX4.IO，我使用的是ubuntu20.04 一、官方环境搭建 1、下载PX4固件 git clone https://github.com/PX4/Firmware.git --recursive 注意：这里会下载Gazebo，和JMAVSim两个虚拟仿真软件，可能会造成下载有点时间长 2、安装依赖 cd ./Firmware bash ./Tools/setup/ubuntu.sh 注意：可以使用--no-nuttx和--no-sim-tools来省略nuttx和/或模
PX4二次开发——编译与启动脚本的修改
qq_36394290的博客
10-16 2551
PX4二次开发——编译和启动脚本的修改 一、在修改之前我们先了解一下目录结构 1.1 总目录结构 ​ 上图，是源码目录 Src：目录是源码目录存放所有的源码，源码的查看都应该在这里。 Mavlink：是MAvlink的库目录，源码要调用这个库，如果我们要修改和添加MAVLINLK消息ID也是在这个目录下面。 NuttX：是NuttX的系统库，如果做二次开发，很少用到这个库。 Cmake：是存放的Cmake编辑脚本文件夹，其中Cmake/Configs是存放的不同硬件的编译脚本，nuttx_mindpx-v
PX4二次开发——uorb订阅
qq_36394290的博客
10-18 1521
PX4二次开发——uorb订阅 一、写在前面 ​ 我们写了一个一个功能的模块，这些模块并不是独立的。模块之间是有数据传递的，这样才能组合到一起实现飞行控制的目的。那么解决模块之间的数据传递的方式就是通过uorb订阅的方式。 ​ 下面举一个例子，包含了飞控串口读取外部传感器数据，飞控开启一个进程读取外部传感器发布一个 UORB 主题，另一个进程订阅前一个进程发布的主题，还有就是订阅到的主题通过 mavlink 消息发送到地面站。走了一个完整的飞控数据链路(UORB 链路和 MAVLINK 链路)。 二、新增一
PX4flow二次开发
03-07
我可以回答这个问题。PX4flow是一种光流传感器，可以用于飞行器的姿态控制和导航。它的二次开发可以根据具体需求进行，例如增加新的功能或者改进性能。需要具备相关的硬件和软件知识，可以参考相关的开发文档和社区讨论。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 6
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值