PX4多旋翼位置控制程序分析 mc_pos_control

1. 概述

在本文中，我们将分析PX4程序的基本流程和关键模块。

2. PX4程序流程图

原图大小太大，无法上传（在我主页下载里面找）

3.PX4主程序梳理：建议对照上图

int MulticopterPositionControl::task_spawn(int argc, char *argv[])
{bool vtol = false;if (argc > 1) {// 判断是否是VTOL飞行器if (strcmp(argv[1], "vtol") == 0) {vtol = true;}// 否则创建的任务将是普通的多旋翼飞行器}MulticopterPositionControl *instance = new MulticopterPositionControl(vtol);if (instance) {// 将新创建的 MulticopterPositionControl 实例的指针存储到 _object 变量中，这样可以方便地在其他函数中获取这个实例的指针_object.store(instance);_task_id = task_id_is_work_queue;// 实例初始化if (instance->init()) {return PX4_OK;}} else {PX4_ERR("alloc failed");}//创建任务失败或初始化失败时，清除实例并释放任务所占用的内存delete instance;// 将 _object 变量中的指针设置为 nullptr，表示实例已经被销毁_object.store(nullptr);_task_id = -1;return PX4_ERROR;
}

MulticopterPositionControl::MulticopterPositionControl(bool vtol) :SuperBlock(nullptr, "MPC"),ModuleParams(nullptr),ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),// 根据vtol来决定发布主题// mc_virtual_attitude_setpoint：是多旋翼无人机特有的控制器；// vehicle_attitude_setpoint：是通用的姿态控制器，适用于各种类型的无人机。_vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),// 速度导数滤波器对象_vel_x_deriv(this, "VELD"),_vel_y_deriv(this, "VELD"),_vel_z_deriv(this, "VELD")
{// 参数更新parameters_update(true);//在进入失控保护降落模式前，设置飞行器需要盘旋的时间0.2s// 如果为false，则表示从当前时间向前设置hysteresis时间；// 如果为true，则表示从当前时间向后设置hysteresis时间。_failsafe_land_hysteresis.set_hysteresis_time_from(false, LOITER_TIME_BEFORE_DESCEND);/* 设置飞行器在执行某些任务时的倾斜角度的限制。setSlewRate函数用于设置倾斜角度的限制斜率，.2f是斜率的值，表示飞行器的倾斜角度变化速率限制为每秒0.2弧度。这里的斜率是指限制器允许的变化速率，例如，在每个时间间隔内限制器可以增加的最大值，也称为限制器的斜率。*/_tilt_limit_slew_rate.setSlewRate(.2f);// 重置 本地位置控制器的目标位置和速度等信息vehicle_local_position_setpoint_s _setpoint{};reset_setpoint_to_nan(_setpoint);
}

vehicle_local_position_setpoint_s 包含了飞行器的位置和速度设定点以及与之相关的加速度，推力等信息，通常被用来在本地位置控制器中传递目标位置和速度等信息，以便控制器对飞行器进行控制。在PX4中，它通常被用于多旋翼和固定翼的位置和速度控制，以及垂直起降（VTOL）飞行器的转换控制。

struct vehicle_local_position_setpoint_s {#endif// 该数据的时间戳uint64_t timestamp; // 目标位置在本地坐标系下的x、y、z坐标float x;float y;float z;// 飞行器当前期望的偏航角度和偏航速度float yaw;float yawspeed;// 目标速度在本地坐标系下的x、y、z轴方向的分量float vx;float vy;float vz;// 本地坐标系下的期望加速度float acceleration[3];// 应该是指沿三个轴上的加加速度（加速度的变化率），用来描述姿态控制的效果（不太确定）float jerk[3];// 飞行器期望的总推力，包含了X，Y和Z方向上的值float thrust[3];// 为了对齐内存而添加的字节，这里用了4个字节。uint8_t _padding0[4]; // required for logger#ifdef __cplusplus#endif
};

实例初始化初始化

bool MulticopterPositionControl::init()
{// 注册了一个_vehicle_local_position_sub(本地位置信息)订阅器的回调函数if (!_local_pos_sub.registerCallback()) {// 注册失败PX4_ERR("vehicle_local_position callback registration failed!");return false;}// 设置为当前系统时间，以便在循环中记录时间间隔_time_stamp_last_loop = hrt_absolute_time();// 开始调度ScheduleNow();return true;
}

void MulticopterPositionControl::Run()
{// 检查是否需要退出程序if (should_exit()) {_local_pos_sub.unregisterCallback(); // 取消本地位置订阅的回调函数exit_and_cleanup();// 执行清理函数return;// 并返回
}
// reschedule backup
ScheduleDelayed(100_ms);// 更新参数
parameters_update(false);// 启动计时
perf_begin(_cycle_perf);

在PX4的代码中，性能测试使用了perf_counter库来进行测量。perf_counter库中包含了一个计时器，能够记录代码执行的时间，并输出给开发者分析。在代码中使用perf_begin()函数启动计时器，perf_end()函数结束计时器，并将结果保存在对应的perf_counter结构体中。这些计时器结果可以被用来优化代码，提高代码的性能。

等待传感器数据

// 等待并更新 飞行器当前的本地位置信息
_local_pos_sub.update(&local_pos)

vehicle_local_position 用于存储飞行器当前的本地位置信息。通常被用来在飞行中获取飞行器的位置和速度等信息，并进行姿态和位置控制，它包含了以下字段：

struct vehicle_local_position_s {#endif// 该数据的时间戳uint64_t timestamp;// 输入到EKF的测量数据的时间戳。uint64_t timestamp_sample;// 参考位置的时间戳uint64_t ref_timestamp;// 参考位置的纬度和经度double ref_lat;double ref_lon;// 飞机在东北天坐标系(本地坐标系)下的位置float x;float y;float z;// 机体坐标系下，飞机相对于位置参考点的偏移量（xy平面）float delta_xy[2];// 飞机相对于位置参考点的高度偏移量（z轴方向）（单位：米）float delta_z;// 在本地坐标系中的速度向量float vx;float vy;float vz;// 飞行器高度的一阶导数：飞机高度的变化率float z_deriv;// 机体坐标系下，飞机相对于位置参考点的速度偏移量float delta_vxy[2];float delta_vz;// 在本地坐标系中的加速度向量float ax;float ay;float az;// 飞机的航向角和相对航向角的偏移量（我觉得增量好理解一点）float heading;float delta_heading;// 参考位置的高度float ref_alt;// 飞机到离地面的距离float dist_bottom;// 水平误差和垂直误差float eph;float epv;// 水平速度误差和垂直速度误差float evh;float evv;// 飞机在水平和竖直方向上的最大速度float vxy_max;float vz_max;// 飞机距离地面的最小和最大高度float hagl_min;float hagl_max;// 位置和速度信息是否可用bool xy_valid;bool z_valid;bool v_xy_valid;bool v_z_valid;// 位置和速度信息的重置计数器uint8_t xy_reset_counter;  // 飞行器在x和y方向上的位置参考点发生变化的次数uint8_t z_reset_counter;uint8_t vxy_reset_counter; // 在z方向上的速度参考点发生变化的次数uint8_t vz_reset_counter;uint8_t heading_reset_counter;// 位置信息是否为全局位置信息和地面距离信息是否有效bool xy_global;bool z_global;bool dist_bottom_valid;// 记录使用的底部距离传感器类型的位掩码uint8_t dist_bottom_sensor_bitfield;uint8_t _padding0[3]; // required for logger#ifdef __cplusplusstatic constexpr uint8_t DIST_BOTTOM_SENSOR_NONE = 0;static constexpr uint8_t DIST_BOTTOM_SENSOR_RANGE = 1;static constexpr uint8_t DIST_BOTTOM_SENSOR_FLOW = 2;#endif
};

注意点

在PX4中，本地坐标系是一个右手系，其中X轴指向东方，Y轴指向北方，Z轴指向上方。这被称为东北天坐标系（ENU）。与此相对的，北东地坐标系（NED）中，X轴指向北方，Y轴指向东方，Z轴指向下方。

在PX4中，使用的是ENU坐标系，这种坐标系在航空领域中非常常见，因为可以直接使用地球经纬度坐标系（即经度、纬度和海拔高度）来定义位置。而在NED坐标系中，经度和纬度需要进行转换，才能用于定义位置。

虽然两种坐标系的坐标轴方向不同，但它们之间可以通过简单的旋转变换相互转换。

// 本地位置数据的时间戳,用 hrt_abstime 类型表示，即系统启动以来经过的微秒数。
const hrt_abstime time_stamp_now = local_pos.timestamp;// 计算两次循环之间的时间差（以微秒为单位）。* 1e-6f 将时间差转换为秒。
// 确保时间差的值在 0.002 秒和 0.04 秒之间
const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f);
_time_stamp_last_loop = time_stamp_now;

这段代码的作用是计算两次循环之间的时间间隔 dt，并确保 dt 的值在一定的范围内，以保证控制器的更新频率适当

// 如果在当前周期内进入故障安全则为ture
const bool was_in_failsafe = _in_failsafe;_in_failsafe = false;// 更新控制模式
vehicle_control_mode_s _control_mode{};
_control_mode_sub.update(&_control_mode);

在PX4中，vehicle_control_mode 用于存储飞行器当前控制模式的各种标志位。这个数据结构通常被用来在飞行中获取飞行器当前的控制模式。在PX4中，它被广泛应用于多旋翼和固定翼的位置、速度和姿态控制，以及垂直起降（VTOL）飞行器的转换控制等任务中。

struct vehicle_control_mode_s {#endifuint64_t timestamp; // 飞行器是否处于解锁状态bool flag_armed;// 是否允许外部手动遥控器覆盖自动控制器的输出bool flag_external_manual_override_ok;// 是否启用多旋翼位置控制bool flag_multicopter_position_control_enabled;// 是否启用手动控制、自动控制bool flag_control_manual_enabled;bool flag_control_auto_enabled;// 否启用Offboard控制、角速度控制bool flag_control_offboard_enabled;bool flag_control_rates_enabled;// 启用了姿态、加速度、速度、位置、高度控制bool flag_control_attitude_enabled;bool flag_control_acceleration_enabled;bool flag_control_velocity_enabled;bool flag_control_position_enabled;bool flag_control_altitude_enabled;// 是否启用爬升率控制、控制终止模式bool flag_control_climb_rate_enabled;bool flag_control_termination_enabled;uint8_t _padding0[2]; // required for logger#ifdef __cplusplus#endif
};

// 更新飞行器当前的着陆检测状态信息
vehicle_land_detected_s _vehicle_land_detected
_vehicle_land_detected_sub.update(&_vehicle_land_detected);

在PX4中，vehicle_land_detected_s 用于存储飞行器当前的着陆检测状态信息。这个数据结构通常被用来在飞行中检测飞行器是否已经着陆，并据此进行相应的控制。

struct vehicle_land_detected_s {#endifuint64_t timestamp;// 飞行器最大允许高度，超过该高度将不会被视为着陆float alt_max;// 表示是否处于自由落体状态bool freefall;// 是否与地面接触bool ground_contact;// 是否可能已经着陆bool maybe_landed;// 是否已经着陆bool landed;// 是否在地面效应区内（地面或水面附近）bool in_ground_effect;// 是否在下降bool in_descend;// 是否有低油门bool has_low_throttle;// 是否垂直移动、是否水平移动bool vertical_movement;bool horizontal_movement;// 是否接近地面或跳过了检查bool close_to_ground_or_skipped_check;uint8_t _padding0[2]; // required for logger#ifdef __cplusplus#endif
}

// 更新飞行器当前的悬停推力估计信息
hover_thrust_estimate_s hte;
if (_hover_thrust_estimate_sub.update(&hte)) {if (hte.valid) {_control.updateHoverThrust(hte.hover_thrust);}}

在PX4中， hover_thrust_estimate_s 通常被用来在飞行中估计飞行器在悬停状态下所需的推力，并据此进行推力控制。

struct hover_thrust_estimate_s {#endifuint64_t timestamp;uint64_t timestamp_sample;// 采样时间戳(用于推力估计的传感器时间戳)// 悬停时的推力估计值、推力估计方差float hover_thrust;float hover_thrust_var;// 估算当前状态下的加速度值、方差、测试比率？？float accel_innov;float accel_innov_var;float accel_innov_test_ratio;// 加速度计噪声方差、当前数据是否有float accel_noise_var;bool valid;uint8_t _padding0[7]; // required for logger#ifdef __cplusplus#endif
};

// 检查当前位置是否有效，将local_pos(当前位置、速度、加速度数据)以向量形式保存到states结构体
PositionControlStates states{set_vehicle_states(local_pos)};

在PX4中，PositionControlStates是一个结构体，用于存储位置控制器的状态信息。

struct PositionControlStates {matrix::Vector3f position;     // 一个三维向量，表示当前飞行器的位置坐标（单位：米）matrix::Vector3f velocity;     // 表示当前飞行器的加速度向量（单位：米/秒^2）matrix::Vector3f acceleration; // 表示当前飞行器的偏航角（单位：弧度）float yaw;
};

这个结构体通常被用来在飞行中更新位置控制器的状态信息，并据此进行位置控制。

例如，在多旋翼的位置控制中，PositionControlStates结构体可以用于存储当前位置、速度和加速度信息，以及当前偏航角信息，然后通过与目标位置、速度和加速度进行比较，计算出相应的控制指令，并将其传递给飞行控制器进行执行。

// 检查一个浮点数是否为有限的数（即不是无穷大或NaN）
constexpr bool PX4_ISFINITE(float x) { return __builtin_isfinite(x); }

 在具体实现上，这个函数使用了GCC/Clang编译器内置的函数__builtin_isfinite()，它会返回一个布尔值，表示传入的浮点数是否是有限的数。由于这个函数被声明为constexpr，因此可以在编译时进行求值，而不需要在运行时进行计算。因此，当PX4中需要检查一个浮点数是否为有限的数时，可以直接调用这个函数，例如：

float x = 1.0 / 0.0;  // x为无穷大
if (PX4_ISFINITE(x)) {// x是有限的数，true
} else {// x是无穷大或NaN，false
}

通过这种方式，PX4可以有效地避免在运行时出现除以零、无穷大或NaN等错误，从而提高了系统的鲁棒性和稳定性。

重置导数以防止恢复速度时出现加速度峰值
_vel_x_deriv.reset();

// 判断多旋翼位置控制是否使能
if (_control_mode.flag_multicopter_position_control_enabled)// 将飞行器当前的轨迹跟踪目标点信息保存到_setpoint结构体（存储本地位置控制器的目标位置和速度等信息）
// 后面是要发布出去的目标点数据
_trajectory_setpoint_sub.update(&_setpoint);

在PX4中，trajectory_setpoint是一个数据结构，用于存储飞行器当前的轨迹跟踪目标点信息。它包含了以下字段：

struct vehicle_local_position_setpoint_s {#endifuint64_t timestamp;// 期望位置的坐标，分别表示在局部东、北、天坐标系下的位置，单位为米float x;float y;float z;// 期望偏航角，期望偏航角速度float yaw;float yawspeed;// 表示当前的目标速度向量float vx;float vy;float vz;// 当前的目标加速度向量、目标加加速度（加速度的变化率）向量、期望推力的向量float acceleration[3];float jerk[3];float thrust[3];uint8_t _padding0[4]; // required for logger#ifdef __cplusplus#endif
}

// 使用任何EKF复位增量调整现有（或旧）设定点
if (_setpoint.timestamp < local_pos.timestamp) {if (local_pos.vxy_reset_counter != _vxy_reset_counter) {_setpoint.vx += local_pos.delta_vxy[0];_setpoint.vy += local_pos.delta_vxy[1];}if (local_pos.vz_reset_counter != _vz_reset_counter) {_setpoint.vz += local_pos.delta_vz;}if (local_pos.xy_reset_counter != _xy_reset_counter) {_setpoint.x += local_pos.delta_xy[0];_setpoint.y += local_pos.delta_xy[1];}if (local_pos.z_reset_counter != _z_reset_counter) {_setpoint.z += local_pos.delta_z;}if (local_pos.heading_reset_counter != _heading_reset_counter) {_setpoint.yaw += local_pos.delta_heading;}}

逐句分析：

if (_setpoint.timestamp < local_pos.timestamp)

判断当前设定点的时间戳是否小于当前本地位置信息的时间戳，确保了设定点的时间戳在本地位置信息之后，即设定点是最新的。如果是最新的设定点，则需要更新其速度信息。

if (local_pos.vxy_reset_counter != _vxy_reset_counter)

如果需要更新速度信息，则判断当前本地位置信息中飞行器在xy方向上的速度参考点是否发生变化，如果发送了变化，则说明飞行器的速度信息已被重置，此时需要将当前本地位置信息中的delta_vxy（表示飞行器速度与参考点速度之间的差值）加入到设定点的速度信息中，以更新设定点的速度信息。

这个更新过程可以保证设定点的速度信息始终是最新的，以更好地实现基于位置控制的飞行。

在这里详解一下vxy_reset_counter：

在PX4中，vxy_reset_counter是一个用于本地速度计算的计数器。本地速度指的是飞行器在当前位置相对于地面的实际速度，计算方法是将IMU测量得到的加速度数据进行积分，得到速度信息。然而，由于积分过程中存在噪声等干扰，会导致速度信息出现漂移，进而影响位置控制的精度。为了解决这个问题，PX4采用了一种基于外部参考点的位置控制方法，即在飞行过程中定期使用GPS等外部设备获得一个参考点，并将该参考点的速度作为参考速度，用于修正飞行器本地速度计算中的漂移误差。

vxy_reset_counter就是用于记录本地速度计算过程中参考点的变化次数的计数器。每当接收到一个新的参考点时，PX4会将vxy_reset_counter加1，表示参考点已经变化了。在更新设定点速度时，如果发现当前的vxy_reset_counter与之前的值不同，就说明参考点已经变化了，此时需要重新计算本地速度与参考速度之间的差值，并将其加入到设定点速度中。这样可以保证设定点速度始终与参考速度保持一致，从而提高位置控制的精度。

而xy_reset_counter;z_reset_counter;vxy_reset_counter;vz_reset_counter;heading_reset_counter;这几个也是同样的意思，用于记录位置控制过程中发生了多少次参考点的变化，都是为了保证位置控制的精度，确保设定点的速度和方向始终与参考速度和方向保持一致。

// update vehicle constraints and handle smooth takeoff
// 更新飞行器约束并处理平稳起飞
_vehicle_constraints_sub.update(&_vehicle_constraints);vehicle_constraints_s _vehicle_constraints

在PX4中，vehicle_constraints_s是一个结构体，用于存储飞行器在不同状态下的运动约束信息。

struct vehicle_constraints_s {#endifuint64_t timestamp;// 在水平、上升、下降方向上的最大速度约束float speed_xy;float speed_up;float speed_down;// 用于指示飞行器是否正在尝试起飞，当为true时表示正在起飞bool want_takeoff;uint8_t _padding0[3]; // required for logger#ifdef __cplusplus#endif
};

例如：

在飞行器起飞和降落时，可能需要限制其在上升和下降速率上的变化范围，以确保起降过程平稳无误。
在执行高速飞行时，可能需要限制其在水平方向上的最大速度，以防止因过快的速度导致控制失效或飞行器失去平衡。
在进行低空飞行时，为了避免与障碍物碰撞，可能需要限制其在x和y方向上的速度和在倾斜角度上的变化范围；
当飞行器在进行着陆时，为了保证安全着陆，可能需要限制其在下降速率上的变化范围

由于不同状态下的运动约束可能不同，因此需要在不同的状态下更新vehicle_constraints_s结构体中的值


// 处理起飞时的上升速度：是否为NAN 还是 大于参数设定值// fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN
// TODO: this should get obsolete once the takeoff limiting moves into the flight tasks
if (!PX4_ISFINITE(_vehicle_constraints.speed_up) || (_vehicle_constraints.speed_up > _param_mpc_z_vel_max_up.get())) {_vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get();
}// 修复以防止起飞坡道倾斜到太高的值或因NAN而卡住
// 待办事项：一旦起飞限制进入飞行任务，这应该会过时

(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,

在PX4中，ParamFloat是一个参数类，用于管理和访问浮点型参数。ParamFloatpx4::params::MPC_Z_VEL_MAX_UP是一个ParamFloat类的实例化对象，它表示名为MPC_Z_VEL_MAX_UP的浮点型参数。

MPC_Z_VEL_MAX_UP是一个参数标签，用于标识该参数的名称和用途。在PX4中，参数可以被用于配置系统的各种属性和行为。例如，MPC_Z_VEL_MAX_UP参数用于配置飞行器在上升方向上的最大速度，单位为米每秒。这个参数可以通过不同的方式进行设置，例如在参数文件中进行配置、通过命令行进行配置、通过Ground Control Station进行配置等。

在PX4中，ParamFloat类的实例化对象可以通过调用getParam()方法来获取当前参数的值，也可以通过调用setParam()方法来修改参数的值。例如，_param_mpc_z_vel_max_up.getParam()可以用于获取MPC_Z_VEL_MAX_UP参数的当前值，_param_mpc_z_vel_max_up.setParam(10.0f)可以用于将MPC_Z_VEL_MAX_UP参数的值修改为10.0。

// Offboard控制模式是否使能
if (_control_mode.flag_control_offboard_enabled) {// 检测飞行器是否需要起飞状态// 飞机已解锁并且当前飞机还停在地面上、在1秒内没有更新控制点，则认为需要起飞bool want_takeoff = _control_mode.flag_armed && _vehicle_land_detected.landed&& hrt_elapsed_time(&_setpoint.timestamp) < 1_s;if (want_takeoff && PX4_ISFINITE(_setpoint.z)// 不用解释了吧&& (_setpoint.z < states.position(2))) { // 期望高度小于当前高度_vehicle_constraints.want_takeoff = true;  // 起飞~~~} else if (want_takeoff && PX4_ISFINITE(_setpoint.vz)&& (_setpoint.vz < 0.f)) { //期望的垂直速度小于0 _vehicle_constraints.want_takeoff = true;  // 起飞~~~} else if (want_takeoff && PX4_ISFINITE(_setpoint.acceleration[2])&& (_setpoint.acceleration[2] < 0.f)) {  //表示期望的垂直加速度小于0_vehicle_constraints.want_takeoff = true;  // 起飞~~~} else {_vehicle_constraints.want_takeoff = false;}// override with defaults // 用默认参数限制在各个方向上的速度_vehicle_constraints.speed_xy = _param_mpc_xy_vel_max.get();_vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get();_vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get();}

hrt_elapsed_time函数：通常用于计算时间差，以便判断某些时间相关的事件是否已经发生或者超时。例如，在判断飞行器是否已经起飞时，可以使用该函数计算当前时刻与起飞指令时刻之间的时间间隔，如果时间间隔超过了某个阈值，就认为飞行器已经起飞。

// handle smooth takeoff 平稳起飞
_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, _vehicle_constraints.want_takeoff,_vehicle_constraints.speed_up, false, time_stamp_now);

详解一下updateTakeoffState函数，处理飞机从未解锁状态到解锁状态，再到飞机起飞升空的处理流程，takeoff.cpp

void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us)
{// 设置一个状态滞后器,在起飞之前给电机一些时间来预热和稳定，从而提高系统的可靠性和稳定性_spoolup_time_hysteresis.set_state_and_update(armed, now_us);switch (_takeoff_state) {case TakeoffState::disarmed:// 起飞状态设置为未解锁状态if (armed) {// 如果检测到飞机已经解锁，则将起飞状态置为spoolup_takeoff_state = TakeoffState::spoolup;// 似乎指的是等待电机和螺旋桨加速到足够转速的状态} else {break;}// FALLTHROUGH case TakeoffState::spoolup:// 监测电机是否已经达到了旋转速度if (_spoolup_time_hysteresis.get_state()) {//电机是否已经达到了旋转速度，表示起飞准备已经完成_takeoff_state = TakeoffState::ready_for_takeoff;} else {break;// 继续等待电机旋转到预设值}// FALLTHROUGHcase TakeoffState::ready_for_takeoff:if (want_takeoff) {// 检测飞行器需要起飞// 设置状态为逐渐增加油门、加速并开始起飞_takeoff_state = TakeoffState::rampup;// 表示的是飞机起飞时的加速度随时间的变化率//初始值被设为0.0，表示加速度为0，随着时间的增加，加速度逐渐增加_takeoff_ramp_progress = 0.f;// 用来控制起飞的过渡} else {break;}// FALLTHROUGHcase TakeoffState::rampup: // 可以理解为飞机正在爬升阶段// 当起飞的加速度达到1时，起飞过渡已经完成，切换到flight状态if (_takeoff_ramp_progress >= 1.f) {_takeoff_state = TakeoffState::flight;} else {break;}// FALLTHROUGHcase TakeoffState::flight:if (landed) {// 检测飞机是否已经着陆，如果已经着陆，则会回到 TakeoffState::ready_for_takeoff 状态_takeoff_state = TakeoffState::ready_for_takeoff;}// 这个过程就是一个循环，一旦检测到飞机需要起飞，状态机就会根据特定的顺序执行各个状态，// 直到飞机达到 flight 状态并成功起飞。break;default:break;}// 根据飞控状态（是否解锁）更新起飞状态，armed 解锁为trueif (armed && skip_takeoff) {             _takeoff_state = TakeoffState::flight; //解锁且跳过起飞跳过起飞过程，但是在这传进的参数是skip_takeoff=false} // 还有一种情况是解锁但未跳过起飞，则维持当前起飞状态不变，等待起飞过程中的状态变化。// TODO: need to consider free fall hereif (!armed) {_takeoff_state = TakeoffState::disarmed;// 未解锁，起飞状态设置为未解锁状态}
}

详解

_spoolup_time_hysteresis.set_state_and_update(armed, now_us)

这段代码是在设置一个状态滞后器 _spoolup_time_hysteresis 的状态和更新时间。它的作用是在起飞之前给电机一些时间来预热和稳定，从而提高系统的可靠性和稳定性。

具体来说，_spoolup_time_hysteresis 是一个状态滞后器对象，它会根据输入的状态（armed）和时间（now_us）更新其内部状态。这个状态滞后器对象会记住上一次的状态，并且在状态发生变化后延迟一段时间才会更新自己的状态，这个延迟时间就是滞后时间。在这段代码中，根据当前的飞行状态（armed）和时间（now_us），状态滞后器会更新内部状态并记录当前时间戳。

滞后器的目的是避免无意义的反应或者跳动，以及将一些状态在一定时间内的稳定化，从而更好的控制电机转速。在 PX4 中，这个状态滞后器 _spoolup_time_hysteresis 用于控制电机的启动时间，从而确保电机转速稳定，并减少飞行器在起飞时因电机启动不充分导致的异常情况。


// 飞行器是否尚未起飞，飞机状态是 爬升阶段
const bool not_taken_off = (_takeoff.getTakeoffState() < TakeoffState::rampup);// 飞行器是否正在飞行
const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight);// 飞行器是否正在飞行但与地面接触，即飞行器已经起飞，但是在降落时尚未完全离开地面
// 这我理解是飞机正处于降落阶段
const bool flying_but_ground_contact = (flying && _vehicle_land_detected.ground_contact);// make sure takeoff ramp is not amended by acceleration feed-forward
// 确保起飞阶段（爬升阶段）的爬升斜率不受加速度前馈的影响，因此将控制指令中的Z轴加速度设为NAN
if (!flying) {   _setpoint.acceleration[2] = NAN;
}// 飞机还没有起飞或者正在降落时，需要避免进行任何控制指令的调整
if (not_taken_off || flying_but_ground_contact) {// 因此将控制指令设置为NAN（不可用）以避免对控制器产生任何干扰reset_setpoint_to_nan(_setpoint);Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust// prevent any integrator windup// 将控制器的积分部分重置，以避免因为状态的改变（例如从起飞阶段到下降阶段）导致积分器饱和，从而对控制效果产生不良影响。_control.resetIntegral();

// limit tilt during takeoff ramupup // 分别设置飞行器在起飞期间和升到一定高度后倾斜角度限制
// 在起飞期间：防止其在空中倾斜过度而失去平衡
// 当飞行器升起到一定高度后，就可以放宽倾斜角度的限制，以便更好地完成任务。
const float tilt_limit_deg = (_takeoff.getTakeoffState() < TakeoffState::flight)? _param_mpc_tiltmax_lnd.get() : _param_mpc_tiltmax_air.get();// 对倾斜角度进行平滑处理，以避免飞行器突然改变倾斜角度而导致不稳定
// 然后将限制后的倾斜角度值设置到控制器中
_control.setTiltLimit(_tilt_limit_slew_rate.update(math::radians(tilt_limit_deg), dt));

在起飞阶段，倾斜角度限制取参数_param_mpc_tiltmax_lnd的值，即地面模式下的最大倾斜角度
在飞行阶段，倾斜角度限制取参数_param_mpc_tiltmax_air的值，即空中模式下的最大倾斜角度

math::radians(tilt_limit_deg) // 角度转弧度
update() 函数接收两个参数，第一个参数是期望的 _tilt_limit 的弧度值，第二个参数是时间步长 dt。
会根据 _tilt_limit 的当前值和给定的弧度值来计算新的 _tilt_limit 值，并确保这个值的变化不会太快，以免出现突变

// 根据起飞状态计算上升速度
const float speed_up = _takeoff.updateRamp(dt,PX4_ISFINITE(_vehicle_constraints.speed_up) ? _vehicle_constraints.speed_up : _param_mpc_z_vel_max_up.get());// 计算飞机垂直下降速度
const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down :_param_mpc_z_vel_max_dn.get();// 计算飞机水平速度
const float speed_horizontal = PX4_ISFINITE(_vehicle_constraints.speed_xy) ? _vehicle_constraints.speed_xy :_param_mpc_xy_vel_max.get();// 设置最小推力，还外起飞：0，已经起飞：系统设置的最小油门值
const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f;// 推力限幅
_control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get());// 水平速度、上升速度、下降速度限幅
_control.setVelocityLimits(math::constrain(speed_horizontal, 0.f, _param_mpc_xy_vel_max.get()),math::min(speed_up, _param_mpc_z_vel_max_up.get()), // takeoff ramp starts with negative velocity limitmath::constrain(speed_down, 0.f, _param_mpc_z_vel_max_dn.get()));

// 设置控制输入（期望的位置、速度、加速度和姿态等信息），用于后续控制器中的计算和决策，进行调整并控制飞行器的运动
_control.setInputSetpoint(_setpoint);// 具体函数实现
void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
{// 飞机应该达到的目标位置_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);// 向量运算// 飞机应该达到的目标速度  _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);// 机应该达到的目标加速度_acc_sp = Vector3f(setpoint.acceleration);// 飞机应该达到的目标偏航角度_yaw_sp = setpoint.yaw;// 飞机应该达到的目标旋转速率_yawspeed_sp = setpoint.yawspeed;
}

// 如果当前的高度控制器没有控制高度
if (!PX4_ISFINITE(_setpoint.z)
// 但是速度发生了变化，那么就需要对速度进行重新设置&& PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON)&& PX4_ISFINITE(local_pos.z_deriv) && local_pos.z_valid && local_pos.v_z_valid) {// 为了得到更加准确的速度值，使用了加权平均的方法
// 具体来说，首先使用局部位置的高度变化率(local_pos.z_deriv)来计算速度，但由于它有一定的偏差，
// 同时还需要考虑当前的速度值，因此需要对这两个速度值进行加权平均float weighting = fminf(fabsf(_setpoint.vz) / _param_mpc_land_speed.get(), 1.f);
// 这样能够让速度的变化更加平滑。states.velocity(2) = local_pos.z_deriv * weighting + local_pos.vz * (1.f - weighting);}// 如果 期望的z轴速度（垂直速度）> 当前的降落速度 ： 加权因子为1，完全使用当前高度的导数来计算速度
//
// 如果 期望的z轴速度（垂直速度）< 当前的降落速度 ： 加权因子就变成了当前速度与 MPC_LAND_SPEED 的比值，那么就需要对这两个速度值进行加权平均

这段代码的作用是更新状态量，其中的if语句块是在特定条件下对状态量进行更新。

在if语句中，首先判断了_setpoint.z是否为有限数（finite number），如果不是有限数，则继续进行条件判断。然后判断_setpoint.vz是否为有限数，并且其绝对值大于FLT_EPSILON（这个值是一个很小的浮点数，用于避免舍入误差），同时判断本地位置（local_pos）中的高度变化率（z_deriv）、高度是否有效（z_valid）以及垂直速度是否有效（v_z_valid）。

如果上述条件全部成立，说明当前的飞行任务需要改变垂直速度，并且不需要控制高度。在这种情况下，代码通过权重因子对当前的垂直速度进行更新，具体来说，会根据当前的降落速度（_param_mpc_land_speed）来决定使用当前高度变化率（z_deriv）和垂直速度（vz）的权重，从而实现平滑过渡。

最终，状态量states.velocity(2)（即垂直速度）的值被更新为这个加权值。

设置控制器当前的状态,其中包括位置、速度、偏航角和速度变化率信息
_control.setState(states);

// Run position control（这儿是关键，主程序流程分析完了，在文章后面分析）if ([_control.update(dt)]) {//如果位置控制器成功更新控制输入并生成新的姿态控制指令，则重置失控保护状态_failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now);} else {// 否则会进入失控保护模式if ((time_stamp_now - _last_warn) > 2_s && _last_warn > 0) {PX4_WARN("invalid setpoints");_last_warn = time_stamp_now;}// 在失控保护模式下，会生成一个失败安全设定点vehicle_local_position_setpoint_s failsafe_setpoint{};// 计算一个安全的控制指令failsafe(time_stamp_now, failsafe_setpoint, states, !was_in_failsafe);// 将约束条件重置为默认值_vehicle_constraints = {0, NAN, NAN, NAN, false, {}};// 设置位置控制器的输入设定点为失败安全设定点，并设置速度限制_control.setInputSetpoint(failsafe_setpoint);_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());// 最后再次调用位置控制器的update()函数来生成新的控制指令_control.update(dt);}

// 将控制器输出的位置和姿态期望值发布到PX4的MAVLink通信协议中// 创建消息数据结构
vehicle_local_position_setpoint_s local_pos_sp{};// 存储期望位置
_control.getLocalPositionSetpoint(local_pos_sp);
// 将时间戳设置为当前系统时间
local_pos_sp.timestamp = hrt_absolute_time();
// 发布消息到MAVLink
_local_pos_sp_pub.publish(local_pos_sp);// Publish attitude setpoint output
vehicle_attitude_setpoint_s attitude_setpoint{};// 存储期望姿态值
_control.getAttitudeSetpoint(attitude_setpoint);
attitude_setpoint.timestamp = hrt_absolute_time();
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);

// 如果多旋翼位置控制没有使能
else { // 需要更新起飞状态 ，否则起飞状态不会被非高度控制模式跳过_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, true, time_stamp_now);}

// 发布起飞状态
const uint8_t takeoff_state = static_cast<uint8_t>(_takeoff.getTakeoffState());// 判断当前的起飞状态和上一次发布时的起飞状态是否一致，或者俯仰限制的斜率是否与上一次一致
if (takeoff_state != _takeoff_status_pub.get().takeoff_state|| !isEqualF(_tilt_limit_slew_rate.getState(), _takeoff_status_pub.get().tilt_limit)) {// 更新 _takeoff_status_pub 话题_takeoff_status_pub.get().takeoff_state = takeoff_state;_takeoff_status_pub.get().tilt_limit = _tilt_limit_slew_rate.getState();_takeoff_status_pub.get().timestamp = hrt_absolute_time();// 并发布_takeoff_status_pub.update();
}// 保存最新的重置计数器
_vxy_reset_counter = local_pos.vxy_reset_counter;
_vz_reset_counter = local_pos.vz_reset_counter;
_xy_reset_counter = local_pos.xy_reset_counter;
_z_reset_counter = local_pos.z_reset_counter;
_heading_reset_counter = local_pos.heading_reset_counter;

// 结束计时器，并将结果保存在对应的perf_counter结构体中
perf_end(_cycle_perf);

4.位置控制和速度控制（包含加速度控制）

bool PositionControl::update(const float dt)
{// 通过检查输入设置点是否有效，判断控制器的输入是否合法const bool valid = (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1)))&& (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1)))&& (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1)));// 位置控制计算：使无人机沿着设定位置以期望速度飞行，并且通过速度限制保证无人机飞行安全。_positionControl();// 速度控制计算_velocityControl(dt);// 对输入的期望偏航速度和期望偏航角进行赋值，如果为 NaN，那么设置为 0_yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f;// 如果期望偏航角为 NaN，那么将其设置为当前偏航角。_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control// 检查输入设置点是否有效 和 更新过程是否成功，判断控制器的更新是否完成return valid && _updateSuccessful();
}

4.1位置控制

void PositionControl::_positionControl()
{// 根据位置误差和位置环P参数计算期望速度（NED系）// 1、位置偏差=（期望位置 - 当前位置）// 2、emult() 函数表示向量元素之间的按元素乘积// 3、期望速度 = 位置偏差 * 比例增益Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p);// 1、将期望速度输出（vel_sp_position）与 速度前馈输入（_vel_sp ）相加，得到总期望//   如果 _vel_sp 和 vel_sp_position 都不为NAN，则相加： _vel_sp += vel_sp_position；//   如果 _vel_sp 是NAN，则不相加： _vel_sp  = vel_sp_position；ControlMath::addIfNotNanVector3f(_vel_sp , vel_sp_position);// 确保位置控制输出（vel_sp_position）中不含NAN，避免出现计算错误// 检查每个分量是否为NaN，如果是则将其替换为零ControlMath::setZeroIfNanVector3f(vel_sp_position);// 将总体期望速度的水平分量限制在指定的范围内// 官方注释：通过在前馈项上优先考虑沿所需位置设定点的速度分量来约束水平速度。没太理解！！！_vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal);// 限制垂直方向的速度幅值在设定的上下限内_vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down);
}

这儿不太理解(_vel_sp - vel_sp_position).xy()这样写

Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
{// 这个函数的作用是给定一个二维向量v0，一个二维向量v1，和一个最大值max，// 返回一个二维向量vf，使得vf等于v0+v1，但vf的模长不超过max。
}

4.2速度控制

void PositionControl::_velocityControl(const float dt)
{// PID velocity control// 计算速度误差Vector3f vel_error = _vel_sp - _vel;// 使用PID控制器计算期望加速度// 速度误差与比例常数向量相乘，产生一个比例调节向量，控制比例控制器的增益，以确保输出的响应对误差的大小有合适的补偿// _vel_int：速度误差的积分项：由每个时间步的误差累积得到 // _vel_dot：速度导数（替代加速度估计）：由每个时间步的速度累积得到// _vel_dot.emult(_gain_vel_d)微分项：_vel_dot与微分常数向量 _gain_vel_d 元素相乘，产生一个微分调节向量// 微分控制器的作用是通过对速度变化率的响应来减少超调和震荡，以使系统的动态响应更加平滑和稳定// 三个部分相加，就得到了加速度的期望值Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);// 速度误差产生的期望加速度和期望加速度前馈叠加，作为总的期望加速度，原理同位置控制ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);// 加速度控制：根据速度设定值和加速度设定值计算期望推力_accelerationControl();// 垂直方向积分器抗饱和:在飞行器的垂直控制中，油门值往往对应着飞行器的垂直速度，因此限制油门值可以起到控制飞行器垂直速度的作用。// 通过限制油门值的范围,同时满足速度误差的方向和油门值的方向一致，// 来限制积分项的增长，从而避免积分饱和// -_lim_thr_min:由于采用的是NED坐标系，即指向地面为正，而升力是向上的，所以是负的if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) ||(_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.0f)) vel_error(2) = 0.f;}// 限制最大垂直推力以保持稳定_thr_sp(2) = math::max(_thr_sp(2), -_lim_thr_max);// 限制水平最大推力/*这个开平方的方法是用来求出无人机在给定最大总推力限制下，能够提供的最大水平推力量的大小。具体而言，我们可以把无人机总推力限制看作一个圆柱体的高度，注意这里：总推力的限制并不一定沿着垂直方向，而是指在所有方向上的推力限制之和。而水平推力限制看作圆柱体的底面积，那么无人机总推力限制和水平推力限制就可以确定这个圆柱体的大小。在这种情况下，我们需要计算的是圆柱体底面的最大面积，也就是能够提供的最大水平推力量。通过勾股定理可以知道，在给定总推力限制和当前沿垂直方向的推力量的情况下，水平推力量的最大值就是圆柱体底面的直径的一半，也就是圆柱体底面积的平方根。因此，我们需要对计算出来的底面积和总推力量分别进行平方，然后取它们的差值的平方根，就能得到最大水平推力量的大小。*/const float thrust_max_squared = _lim_thr_max * _lim_thr_max;const float thrust_z_squared = _thr_sp(2) * _thr_sp(2);const float thrust_max_xy_squared = thrust_max_squared - thrust_z_squared;float thrust_max_xy = 0;// 开方求最大水平推力if (thrust_max_xy_squared > 0) {thrust_max_xy = sqrtf(thrust_max_xy_squared);}// 水平方向推力限幅const Vector2f thrust_sp_xy(_thr_sp); // 取前两个元素// 表示给定的推力向量在水平方向上的投影长度，即水平方向的推力大小const float thrust_sp_xy_norm = thrust_sp_xy.norm(); // 计算模长// 相当于将期望水平推力向量投影到一个半径为thrust_max_xy的圆内，以限制其大小if (thrust_sp_xy_norm > thrust_max_xy) {// 将期望推力向量进行归一化，并乘以最大水平推力限制// 归一化：thrust_sp_xy / thrust_sp_xy_norm（将该向量除以它的模长，使得该向量的长度为1）// 将归一化后的向量乘以限制的最大推力，得到限制后的水平方向推力向量// 并将其赋值给 _thr_sp 中的前两个元素_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;}//tracking Anti-Windup 控制// 水平方向上的加速度期望值 = 期望推力 / 悬停推力 * 重力加速度const Vector2f acc_sp_xy_limited = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);// 限制系数: 在水平方向上的比例增益值的两倍的倒数const float arw_gain = 2.f / _gain_vel_p(0);// 水平方向上的速度误差 = 原来的速度误差 - [限制系数 * （期望加速度 - 在水平方向上的加速度期望值)]// 就得到了在水平方向上的跟踪误差。vel_error.xy() = Vector2f(vel_error) - (arw_gain * (Vector2f(_acc_sp) - acc_sp_xy_limited));// 确保输出不含NAN更新速度控制积分项ControlMath::setZeroIfNanVector3f(vel_error);// 更新速度控制积分项_vel_int += vel_error.emult(_gain_vel_i) * dt;// 积分限幅：限制垂直速度（z轴方向）值的绝对值不能超过（9.8 m/s²），并保持其正负性与原来相同。// 使用 sign() 函数确定积分器中储存的垂直速度值的正负性// 防止积分器中储存的垂直速度值过大或过小，导致控制效果不佳或系统不稳定。_vel_int(2) = math::min(fabsf(_vel_int(2)), CONSTANTS_ONE_G) * sign(_vel_int(2));
}

4.3加速度控制

void PositionControl::_accelerationControl()
{// 计算期望的机身Z轴方向（应该朝向重力加速度的反方向）// 这里假设垂直方向只有重力加速度，通过normalized（）函数将合力向量单位化，以确保其模长为1Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();// 将姿态限制在最大倾角内，以确保无人机的稳定性// 如何限制：机体Z轴向量和单位化的地理Z轴向量进行点乘，求得两个向量的余弦，再求反余弦得到倾斜角度，然后对角度进行限幅ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);// 实际需要提供的推力大小 = 期望推力大小 - 悬停推力float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;// Vector3f(0, 0, 1)是一个指向世界坐标系中竖直向上的单位向量。// body_z是一个指向机体坐标系中竖直向上的单位向量。// 两个向量点积运算得到它们在空间中的夹角的余弦值。因为两个向量都是单位向量，它们的点积就是它们夹角的余弦值// 总推力 / 竖直向上和机体z轴向之间的夹角的余弦值 = 机体z轴方向上的总推力（可以画图理解，比较简单）collective_thrust /= (Vector3f(0, 0, 1).dot(body_z));// 总推力限幅collective_thrust = math::min(collective_thrust, -_lim_thr_min);// 将机体z轴方向上的总推力投影回世界坐标系中，得到期望的推力向量_thr_sp// 注意body_z是单位向量_thr_sp = body_z * collective_thrust;
}