一、前言
首先，我们要清楚的我们的需求，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 ，那么就认为它们两个是相等的。