飞控简析-从入门到跑路 第二章PX4的位置控制(1)

一、前言

首先，我们要清楚的我们的需求，PX4的位置控制需要完成什么样的工作。位置控制需要完成的是，从期望位置得到期望姿态的一个过程，然后把期望姿态传递给姿态控制模块，所以位置控制的输入是期望位置，输出是期望姿态（当然可能还需要其他信息）。然后，PX4的位置又可以分为两种，一种是自动控制，即地面站规划航线，无人机自己飞，第二种是手动控制，即遥控器控制期望位置（或者叫GPS定点模式）。这两种控制需要分开处理。下面我们来看下代码。

首先下载PX41.5.4的源代码，具体方法可以查看https://blog.csdn.net/weixin_38693938/article/details/83794666

解压文件后，定位到Firmware-master\src\modules\mc_pos_control\mc_pos_control_main.cpp

二、源码分析

1.辅助代码

首先，找到位置控制的主函数MulticopterPositionControl::task_main()，它位于文件的第1304行。

	/*
	 * do subscriptions
	 */
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
	_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
	_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));

首先代码会进行一大堆的订阅，用来更新无人机的一些状态和数据。这些可以跳过。

	parameters_update(true);

	/* initialize values of critical structs until first regular update */
	_arming.armed = false;

	/* get an initial update for all sensor and status data */
	poll_subscriptions();

	/* We really need to know from the beginning if we're landed or in-air. */
	orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);

	bool reset_int_z = true;
	bool reset_int_z_manual = false;
	bool reset_int_xy = true;
	bool reset_yaw_sp = true;
	bool was_armed = false;

	hrt_abstime t_prev = 0;

	math::Vector<3> thrust_int;
	thrust_int.zero();

	// Let's be safe and have the landing gear down by default
	_att_sp.landing_gear = -1.0f;


	matrix::Dcmf R;
	R.identity();

然后，进行参数初始化更新，还有一些变量的初始化工作，这些也可以跳过。上述代码都是初始化的代码，正常工作后循环运行是不会执行到这里的。

	px4_pollfd_struct_t fds[1];

	fds[0].fd = _local_pos_sub;
	fds[0].events = POLLIN;

上述代码是位置控制的主函数task_main的唤醒条件。使用操作系统的工程，当任务代码执行完成后，都会进入挂起状态，当满足一定的条件后，才会继续执行，而位置控制的唤醒条件就是更新位置信息的时候就会唤醒函数。

	while (!_task_should_exit) {
		/* wait for up to 20ms for data */
		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 20);

		/* timed out - periodic check for _task_should_exit */
		if (pret == 0) {
			// Go through the loop anyway to copy manual input at 50 Hz.
		}

		/* this is undesirable but not much we can do */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			continue;
		}

		poll_subscriptions();

		parameters_update(false);

		hrt_abstime t = hrt_absolute_time();
		float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
		t_prev = t;

		// set dt for control blocks
		setDt(dt);

然后进入函数的唤醒阶段，当fds更新时，函数就会被唤醒，最大的等待时间是20ms，超过20ms函数也会被唤醒。函数被唤醒后，就开始订阅位置，姿态等数据，然后更新参数，计算两次位置控制的间隔时间。

2.主要代码

		if (_control_mode.flag_armed && !was_armed) {
			/* reset setpoints and integrals on arming */
			_reset_pos_sp = true;
			_reset_alt_sp = true;
			_do_reset_alt_pos_flag = true;
			_vel_sp_prev.zero();
			reset_int_z = true;
			reset_int_xy = true;
			reset_yaw_sp = true;
		}

		/* reset yaw and altitude setpoint for VTOL which are in fw mode */
		if (_vehicle_status.is_vtol) {
			if (!_vehicle_status.is_rotary_wing) {
				reset_yaw_sp = true;
				_reset_alt_sp = true;
			}
		}

		//Update previous arming state
		was_armed = _control_mode.flag_armed;

		update_ref();

首先，如果是刚解锁，那么需要重置位置期望值，高度期望值，PID控制的积分等。如果无人机是垂直起降固定翼VTOL且处于固定翼FW模式下，需要重置航向期望值和高度期望值。接着更新was_armed的标志位，重置位置坐标系的原点。正常我们无人机描述的位置是GPS信息，即无人机的经纬度，但是经过EKF或者INAV进行数据的融合后，会把经纬度坐标转换为直角坐标系，这个用来描述位置的坐标系也是北东天坐标系，既然是一个坐标系，那么必然会有一个原点，这个原点在PX4中就被称为ref。这个原点一般是上电的时候就采集好了的。

	/* Update velocity derivative,
		 * independent of the current flight mode
		 */
		if (_local_pos.timestamp > 0) {

			if (PX4_ISFINITE(_local_pos.x) &&
			    PX4_ISFINITE(_local_pos.y) &&
			    PX4_ISFINITE(_local_pos.z)) {

				_pos(0) = _local_pos.x;
				_pos(1) = _local_pos.y;

				if (_params.alt_mode == 1 && _local_pos.dist_bottom_valid) {
					_pos(2) = -_local_pos.dist_bottom;

				} else {
					_pos(2) = _local_pos.z;
				}
			}

			if (PX4_ISFINITE(_local_pos.vx) &&
			    PX4_ISFINITE(_local_pos.vy) &&
			    PX4_ISFINITE(_local_pos.vz)) {

				_vel(0) = _local_pos.vx;
				_vel(1) = _local_pos.vy;

				if (_params.alt_mode == 1 && _local_pos.dist_bottom_valid) {
					_vel(2) = -_local_pos.dist_bottom_rate;

				} else {
					_vel(2) = _local_pos.vz;
				}
			}

			_vel_err_d(0) = _vel_x_deriv.update(-_vel(0));
			_vel_err_d(1) = _vel_y_deriv.update(-_vel(1));
			_vel_err_d(2) = _vel_z_deriv.update(-_vel(2));
		}

然后开始计算速度的微分值，即无人机的实际加速度。首先为_pos()变量赋值，对应x,y,z的坐标，然后给_vel()变量赋值，对应无人机在x,y,z三个方向上的速度，然后调用更新函数求出速度的微分。其中dist_bottom是激光测距或者超声波测距得到的值，然后数值有效且参数设置了就会采用它们测出来的值，否则就是加速度，气压计，GPS等传感器融合出来的高度值。

		if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) {
			_pos_hold_engaged = false;
		}

		if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) {
			_alt_hold_engaged = false;
		}

然后如果不是手动模式或者不允许位置控制、高度控制就置零水平位置，垂直位置的保持标记。在我们用遥控器把飞控切到ALTCTL(高度控制)或者POSCTL(位置控制)的时候，当遥杆回到中位，无人机就会记录当前位置作为期望值，如果不是手动模式，相应的标志位就要清零，上面的代码就是清零标志的。

		if (_control_mode.flag_control_altitude_enabled ||
		    _control_mode.flag_control_position_enabled ||
		    _control_mode.flag_control_climb_rate_enabled ||
		    _control_mode.flag_control_velocity_enabled ||
		    _control_mode.flag_control_acceleration_enabled)

如果允许高度控制/位置控制/爬升率控制/速度控制/加速度控制才进入主代码，否则就会清除相关标志位。

			_vel_ff.zero();

			/* by default, run position/altitude controller. the control_* functions
			 * can disable this and run velocity controllers directly in this cycle */
			_run_pos_control = true;
			_run_alt_control = true;

			/* select control source */
			if (_control_mode.flag_control_manual_enabled) {
				/* manual control */
				control_manual(dt);

			} else if (_control_mode.flag_control_offboard_enabled) {
				/* offboard control */
				control_offboard(dt);
				_mode_auto = false;

			} else {
				/* AUTO */
				control_auto(dt);
			}

然后，首先清除变量_vel_ff，该变量1.5.4版本里面没有用到......。然后给标志位置一，默认运行高度控制和位置控制。如果是手动控制，就运行control_manual(dt)，如果是仿真，就运行control_offboard(dt)，如果是自动控制，就运行control_auto(dt)。手动控制与自动控制是我们最需要关注的两个点。

3.手动控制control_manual

	/* Entering manual control from non-manual control mode, reset alt/pos setpoints */
	if (_mode_auto) {
		_mode_auto = false;

		/* Reset alt pos flags if resetting is enabled */
		if (_do_reset_alt_pos_flag) {
			_reset_pos_sp = true;
			_reset_alt_sp = true;
		}
	}

首先，把标志位_mode_auto清零，然后判断是否需要重置位置，高度的期望值。

	math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range
	req_vel_sp.zero();

	if (_control_mode.flag_control_altitude_enabled) {
		/* set vertical velocity setpoint with throttle stick */
		req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy); // D
	}

	if (_control_mode.flag_control_position_enabled) {
		/* set horizontal velocity setpoint with roll/pitch stick */
		req_vel_sp(0) = _manual.x;
		req_vel_sp(1) = _manual.y;
	}

采集遥控器的位置，可以看到，x,y,z分别代表着俯仰，横滚，油门。

	if (_control_mode.flag_control_altitude_enabled) {
		/* reset alt setpoint to current altitude if needed */
		reset_alt_sp();
	}

	if (_control_mode.flag_control_position_enabled) {
		/* reset position setpoint to current position if needed */
		reset_pos_sp();
	}

然后根据相关标志决定，是否需要清零期望值。

	/* limit velocity setpoint */
	float req_vel_sp_norm = req_vel_sp.length();

	if (req_vel_sp_norm > 1.0f) {
		req_vel_sp /= req_vel_sp_norm;
	}

对遥控器的值进行归一化处理。

	/* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */
	math::Matrix<3, 3> R_yaw_sp;
	R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
	math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(
			_params.vel_cruise); // in NED and scaled to actual velocity

用遥控器俯仰，横滚，油门的杆量的比例乘以巡航速度，在把速度向量转换到大地坐标系内。这一步是什么意思呢？我们知道，在GPS定点模式下，我们推俯仰，飞机就会往机头方向飞行，该步就是把实际需要飞行速度重新转换到NED大地坐标系下。有一点需要注意，如果是自己写程序，_att_sp.yaw_body最好更换为实际的航向值，因为当航向期望值与实际航向值相差过大，很容易出现速度分解出错的问题。

	/*
	 * assisted velocity mode: user controls velocity, but if	velocity is small enough, position
	 * hold is activated for the corresponding axis
	 */

	/* horizontal axes */
	if (_control_mode.flag_control_position_enabled) {
		/* check for pos. hold */
		if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) {
			if (!_pos_hold_engaged) {

				float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1));

				if (_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy) {
					/* reset position setpoint to have smooth transition from velocity control to position control */
					_pos_hold_engaged = true;
					_pos_sp(0) = _pos(0);
					_pos_sp(1) = _pos(1);

				} else {
					_pos_hold_engaged = false;
				}
			}

		} else {
			_pos_hold_engaged = false;
		}

		/* set requested velocity setpoint */
		if (!_pos_hold_engaged) {
			_pos_sp(0) = _pos(0);
			_pos_sp(1) = _pos(1);
			_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
			_vel_sp(0) = req_vel_sp_scaled(0);
			_vel_sp(1) = req_vel_sp_scaled(1);
		}
	}

代码首先判断，期望速度是否运行位置控制，如果运行，再判断期望速度是否小于0.1m/s，或者判断实际速度是否小于0.8m/s。如果期望的XY速度小于0.1m/s，或者实际XY的合速度小于0.8m/s，就可以进入位置保持模式，此时直接将实际位置赋值给期望位置。如果不满足上述条件，就是把当前位置赋值给位置期望值，把遥控器的期望速度赋值给期望速度，要注意_run_pos_control这个变量，当它为false时，是不会根据期望位置计算期望速度的，也就是说，不满足前面的条件下，无人机会直接进入速度控制，期望位置只是个摆设而已。这也是比较符合逻辑的，当速度比较慢的时候，无人机用位置控制比较稳定，当无人机速度比较快的速度，直接控制速度会比较稳定。

	/* vertical axis */
	if (_control_mode.flag_control_altitude_enabled) {
		/* check for pos. hold */
		if (fabsf(req_vel_sp(2)) < FLT_EPSILON) {
			if (!_alt_hold_engaged) {
				if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) {
					/* reset position setpoint to have smooth transition from velocity control to position control */
					_alt_hold_engaged = true;
					_pos_sp(2) = _pos(2);

				} else {
					_alt_hold_engaged = false;
				}
			}

		} else {
			_alt_hold_engaged = false;
			_pos_sp(2) = _pos(2);
		}

		/* set requested velocity setpoint */
		if (!_alt_hold_engaged) {
			_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
			_vel_sp(2) = req_vel_sp_scaled(2);
		}
	

垂向速度的控制和水平是差不多的，首先判断标志位是否运行高度控制，如果运行，再判断期望速度是否小于FLT_EPSILON，FLT_EPSILON代表着最小的浮点数，比它还小的就是0了。然后判断期望速度是否小于FLT_EPSILON。如果期望的垂向速度小于FLT_EPSILON，或者实际垂向速度小于0.6，就可以进入高度保持模式，此时直接将实际高度赋值给期望高度。如果不满足上述条件，就是把实际高度赋值给期望高度，把遥控器的期望速度赋值给期望速度。同样的，要注意_run_alt_control这个变量，当它为false时，是不会根据期望高度计算期望垂向速度的，也就是说，不满足前面的条件下，无人机会直接进入垂向速度控制，期望高度只是个摆设而已。

至此，control_manual就解析完了，我们已经获得了x,y,z三个轴的期望位置，如果不是位置保持模式，我们还得到了三个轴的期望速度（此时没有位置控制，只有速度控制）。

4.FLT_EPSILON

首先有#define FLT_EPSILON  1.192092896e-07F // smallest such that 1.0+FLT_EPSILON != 1.0

据说，这个数是为了解决计算机判断1.0和10.0/10.0之间的误差而存在的，计算机或者单片机存储浮点数是有误差，所以如果两个浮点数之间的差小于FLT_EPSILON ，那么就认为它们两个是相等的。