飞控简析-从入门到跑路第二章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 blockssetDt(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 statewas_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 rangereq_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 ，那么就认为它们两个是相等的。