pixhawk mc_pos_control.cpp源码解读

好久没跟新blog了，这段时期边调试边看程序，所以有点慢。要开始着手调试了。。

这篇blog是顺着上一篇pixhawk 整体架构的认识写的，接下来看程序的话，打算把各个功能模块理解一遍，先从mc_pos_control.cpp看起。

先提一下pixhawk的整体逻辑：

commander和navigator产生期望位置

position_estimator是当前位置

通过pos_ctrl产生期望姿态

attitude_estimator是当前姿态

通过att_ctrl产生pwm的数值

最后通过mixer和motor_driver控制4个电机

pos_ctrl的总体逻辑是：

（1）copy  commander和navigator产生的期望位置-----_pos_sp_triplet结构体

（2）产生位置/速度设定值(期望值)-----_pos_sp<3>向量和_vel_sp<3>向量

（3）产生可利用的速度设定值(期望值)-----_vel_sp<3>向量


（4）产生可利用的推力定值(期望值)-----thrust_sp<3>向量


（5）根据推力向量计算姿态设定值(期望姿态)-----q_sp四元数矩阵和R_sp旋转矩阵


（6）将之前程序得到的各种信息填充_local_pos_sp结构体，并发布出去-----_local_pos_sp（第2、3步得到的）


（7）根据具体应用更改之前得到的姿态设定值(期望姿态)，并发布出去-----_att_sp（第5步得到的）

那么，直接贴代码了，代码中有详细注释（比之前要更详细点，里面细节逻辑太多了，还没完全理清）

先是main代码，再是control_manual(dt);control_offboard(dt);control_auto(dt);函数，再是map_projection_project()函数，最后是常用矩阵和向量函数

task_main()

void
MulticopterPositionControl::task_main()
{

_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);

/*
* do subscriptions
*/
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_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();

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();

math::Matrix<3, 3> R;
R.identity();

/* wakeup source */
px4_pollfd_struct_t fds[1];

fds[0].fd = _local_pos_sub;//主要是判断是否有当地位置跟新
fds[0].events = POLLIN;//#define POLLIN       (0x01)
///////////////////////大循环//////////////////////////////
while (!_task_should_exit) {
/*****************第一部分：一系列的准备工作*****************/
/* wait for up to 500ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);

/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}

/* 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;//跟新t_prev

// set dt for control blocks   给控制块设置dt(这个是操作系统层的)
setDt(dt);
/////////////////////对解锁状态进行判断之后复位位置和高度设置/////////////////
if (_control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
_reset_pos_sp = true;
_reset_alt_sp = true;
_vel_sp_prev.zero();
reset_int_z = true;
reset_int_xy = true;
reset_yaw_sp = true;
}
////////////////固定翼模式为垂直起降控制复位yaw和高度设置/////////////////////////
/* 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();//跟新一些地坐标xyz方向基准值
//////////////////将之前获取的值赋给mc_pos_control_main.cpp里的变量///////////////
//////////////////独立于当前模式跟新加速度//////////////////////
/* 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)) {
/*ISFINITE是判断是否为有限数*/
_pos(0) = _local_pos.x;
_pos(1) = _local_pos.y;
_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;
_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));
/*	math::Vector<3> _vel_err_d;		//< derivative of current velocity */
/*  _vel_err_d当前速度的导数*/
}
////////////非手动模式下或者不需要位置高度控制的情况下，复位水平和垂直位置hold的标志位///////////////
////////////以下的_control_mode.xxxxxx均来自commander.cpp///////////////////////
// reset the horizontal and vertical position hold flags for non-manual modes
// or if position / altitude is not controlled
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;
}
/*****************第二部分：这部分应该就是控制位置控制逻辑的集中体现了*****************/
////////高度控制、位置控制、爬升速率控制、速度控制任意一个使能则进入以下这段程序//////////
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) {

_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;//标志位
/*****************第二部分第一步：产生位置/速度设定值(期望值)*****************/
////////////选择控制源是手动、机外(offboard)、还是自动控制，产生位置/速度设定值(期望值)//////////////////
////////////这部分的三个函数具体会在下面展开/////////////////////
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
control_manual(dt);
_mode_auto = false;

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

} else {
/* AUTO */
control_auto(dt);
}
/*****************第二部分第二步：产生姿态设定值(期望值)*****************/
////////////////////vtol不用管，它是用于固定翼的///////////////////
/* weather-vane mode for vtol: disable yaw control */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.disable_mc_yaw_control == true) {
_att_sp.disable_mc_yaw_control = true;

} else {
/* reset in case of setpoint updates */
_att_sp.disable_mc_yaw_control = false;
}
////////////////////工作在手动控制失能&&当前位置设定值合法&&当前位置设定值的类型是空闲状态下///////////////
////////////////////////////那么不运行控制器，并且设置油门为0/////////////////////////
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
/*	static const uint8_t SETPOINT_TYPE_POSITION = 0;
*	static const uint8_t SETPOINT_TYPE_VELOCITY = 1;
*	static const uint8_t SETPOINT_TYPE_LOITER = 2;
*	static const uint8_t SETPOINT_TYPE_TAKEOFF = 3;
*	static const uint8_t SETPOINT_TYPE_LAND = 4;
*	static const uint8_t SETPOINT_TYPE_IDLE = 5;
*	static const uint8_t SETPOINT_TYPE_OFFBOARD = 6;
*/
R.identity();//R矩阵单位化
/*	在大循环外定义了R矩阵
*	math::Matrix<3, 3> R;
*	R.identity();
*
* Firmware/src/lib/mathlib/math/Matrix.hpp
* set identity matrix单位矩阵
*
* void identity(void) {
*	memset(data, 0, sizeof(data));
*	unsigned int n = (M < N) ? M : N;
*
*	for (unsigned int i = 0; i < n; i++)
*		data[i][i] = 1;
* }
*/
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
//将姿态设定值的R_body[9]数组复制到R矩阵中
_att_sp.R_valid = true;

_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _yaw;
_att_sp.thrust = 0.0f;

_att_sp.timestamp = hrt_absolute_time();
//////////////////////此处发布的姿态设定值不是正常使用的，都被置为0了//////////////////////////
/* publish attitude setpoint */
if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);

} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}

}
////////////////手动控制使能&&着陆使能状态下///////////////////
else if (_control_mode.flag_control_manual_enabled
&& _vehicle_status.condition_landed) {
//////不运行控制器，当着落的时候(所以位置和高度设定值等复位)//////
/* don't run controller when landed */
_reset_pos_sp = true;
_reset_alt_sp = true;
_mode_auto = false;
reset_int_z = true;
reset_int_xy = true;

R.identity();//R矩阵单位化
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
//将姿态设定值的R_body[9]数组复制到R矩阵中
_att_sp.R_valid = true;

_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _yaw;
_att_sp.thrust = 0.0f;

_att_sp.timestamp = hrt_absolute_time();
//////////////////这里发布的姿态设定值是和着陆模式有关的，并不是飞行的姿态设定值///////////////
/* publish attitude setpoint */
if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);

} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}

}
/*****************第二部分第二步的重点：产生姿态设定值(期望值)*****************/
///////////////////这段程序应该才是发布正常飞行状态的姿态设定值//////////////////
///////////////////运行位置和高度控制器(否则采用已经计算出来的速度设定值)//////////////
else {
/****************第二部分第二步的重点(1):产生可利用的速度设定值(期望值)*******************/
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if (_run_pos_control) {
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
}

if (_run_alt_control) {
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
}
/*	刚进入大循环时赋值，进行选择
*	_run_pos_control = true;//标志位
*	_run_alt_control = true;//标志位
*
*	_pos(n)就是之前orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
*	_pos(0) = _local_pos.x;
*	_pos(1) = _local_pos.y;
*	_pos(2) = _local_pos.z;
*
*	_pos_sp就是之前control_manual(dt);control_offboard(dt);control_auto(dt);的输出值
*/
/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
_vel_sp(1) * _vel_sp(1));

if (vel_norm_xy > _params.vel_max(0)) {
/* note assumes vel_max(0) == vel_max(1) */
_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
}
//////////////////以下是设定垂直速度///////////////
/* make sure velocity setpoint is saturated in z*/
float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2));

if (vel_norm_z > _params.vel_max(2)) {
_vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z;
}

if (!_control_mode.flag_control_position_enabled) {
_reset_pos_sp = true;
}

if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
}

if (!_control_mode.flag_control_velocity_enabled) {
_vel_sp_prev(0) = _vel(0);
_vel_sp_prev(1) = _vel(1);
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
control_vel_enabled_prev = false;
}

if (!_control_mode.flag_control_climb_rate_enabled) {
_vel_sp(2) = 0.0f;
}
/////////////////以下是起飞的垂直速度设定/////////////////
/* use constant descend rate when landing, ignore altitude setpoint */
/* 当着陆时，忽略高度设定值，用恒定的速度下降 */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
}

/* special thrust setpoint generation for takeoff from ground */
/* 起飞时产生专用的推力设定值 */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
&& _control_mode.flag_armed) {

// check if we are not already in air.
// if yes then we don't need a jumped takeoff anymore
// 检查飞机是否在空中
if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) {
// 是否在空中
_takeoff_jumped = true;
}

if (!_takeoff_jumped) {
// ramp thrust setpoint up
// 阶梯式的提高推力设定值
if (_vel(2) > -(_params.tko_speed / 2.0f)) {
_takeoff_thrust_sp += 0.5f * dt;
_vel_sp.zero();
_vel_prev.zero();

} else {
// copter has reached our takeoff speed. split the thrust setpoint up
// into an integral part and into a P part
// 飞行器已达到起飞速度。将推力设定值分为积分和比例部分
thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2));
thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max);//限幅
_vel_sp_prev(2) = -_params.tko_speed;
_takeoff_jumped = true;
reset_int_z = false;
}
}

if (_takeoff_jumped) {
_vel_sp(2) = -_params.tko_speed;
}
///////////////以上是起飞垂直速度设定/////////////////////
} else {
_takeoff_jumped = false;
_takeoff_thrust_sp = 0.0f;
}
/////////////////以上是起飞的垂直速度设定/////////////////

// limit total horizontal acceleration
// 限制水平加速度
math::Vector<2> acc_hor;
acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;

if (acc_hor.length() > _params.acc_hor_max) {
acc_hor.normalize();//标准化
acc_hor *= _params.acc_hor_max;//限幅完成
math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev;//acc*dt+v_prev_sp
_vel_sp(0) = vel_sp_hor(0);//修改限幅后的水平速度设定
_vel_sp(1) = vel_sp_hor(1);//修改限幅后的水平速度设定
}

// limit vertical acceleration
// 限制垂直加速度
float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;

if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
acc_v /= fabsf(acc_v);//标准化
_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);//acc*dt+v_prev_sp
}

_vel_sp_prev = _vel_sp;

_global_vel_sp.vx = _vel_sp(0);
_global_vel_sp.vy = _vel_sp(1);
_global_vel_sp.vz = _vel_sp(2);

/* publish velocity setpoint */
if (_global_vel_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);

} else {
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
}
/************************************************************************************************
*orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
* 经过control_manual(dt);control_offboard(dt);control_auto(dt);输出pos_sp
* 经过上部分输出_vel_sp
* 发布_global_vel_sp
************************************************************************************************/

/****************第二部分第二步的重点(2):产生可利用的推力定值(期望值)*******************/
////////////////////爬升速率控制使能||水平速度控制使能///////////////////////
if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
/* reset integrals if needed */
if (_control_mode.flag_control_climb_rate_enabled) {
//爬升速率控制使能
if (reset_int_z) {
reset_int_z = false;
float i = _params.thr_min;

if (reset_int_z_manual) {
i = _params.thr_hover;

if (i < _params.thr_min) {
i = _params.thr_min;

} else if (i > _params.thr_max) {
i = _params.thr_max;
}
}

thrust_int(2) = -i;
}

} else {
reset_int_z = true;
}

if (_control_mode.flag_control_velocity_enabled) {
//水平速度控制使能
if (reset_int_xy) {
reset_int_xy = false;
thrust_int(0) = 0.0f;
thrust_int(1) = 0.0f;
}

} else {
reset_int_xy = true;
}

/* velocity error */
math::Vector<3> vel_err = _vel_sp - _vel;
/* 	_vel是实际飞行器的速度
* 	_vel(0) = _local_pos.vx;
* 	_vel(1) = _local_pos.vy;
*	_vel(2) = _local_pos.vz;
*	struct vehicle_local_position_s			_local_pos;
*	orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
*/
// check if we have switched from a non-velocity controlled mode into a velocity controlled mode
// if yes, then correct xy velocity setpoint such that the attitude setpoint is continuous
// 检查我们是否将非速度控制模式转变成速度控制模式，如果是，那么矫正xy速度设定值，以便姿态设定值是连续的
if (!control_vel_enabled_prev && _control_mode.flag_control_velocity_enabled) {

// choose velocity xyz setpoint such that the resulting thrust setpoint has the direction
// given by the last attitude setpoint
//矫正xy速度设定值
_vel_sp(0) = _vel(0) + (-PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust - thrust_int(0) - _vel_err_d(0) * _params.vel_d(0)) / _params.vel_p(0);
_vel_sp(1) = _vel(1) + (-PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust - thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1);
_vel_sp(2) = _vel(2) + (-PX4_R(_att_sp.R_body, 2, 2) * _att_sp.thrust - thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2);
_vel_sp_prev(0) = _vel_sp(0);
_vel_sp_prev(1) = _vel_sp(1);
_vel_sp_prev(2) = _vel_sp(2);
control_vel_enabled_prev = true;

// compute updated velocity error
//用矫正后的速度设定值-实际速度，跟新速度误差
vel_err = _vel_sp - _vel;
}

/* thrust vector in NED frame */
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;
//推力设定值(三维)=速度差*P+速度差的差*D+积分
/*********************************************************
************上部分就将设定速度转变成设定推力*************
*********************************************************/
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
&& !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) {
// for jumped takeoffs use special thrust setpoint calculated above
thrust_sp.zero();
thrust_sp(2) = -_takeoff_thrust_sp;
}

if (!_control_mode.flag_control_velocity_enabled) {
thrust_sp(0) = 0.0f;
thrust_arrayssp(1) = 0.0f;
}

if (!_control_mode.flag_control_climb_rate_enabled) {
thrust_sp(2) = 0.0f;
}

/* limit thrust vector and check for saturation */
/* 限制推力大小，检查是否饱和 */
bool saturation_xy = false;
bool saturation_z = false;

/* limit min lift */
//限制最小升力
float thr_min = _params.thr_min;

if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
/* don't allow downside thrust direction in manual attitude mode */
//不允许下降推力在手动姿态模式
thr_min = 0.0f;
}

float thrust_abs = thrust_sp.length();
float tilt_max = _params.tilt_max_air;
float thr_max = _params.thr_max;
/* filter vel_z over 1/8sec */
_vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2);//垂直速度低通滤波
/* filter vel_z change over 1/8sec */
float vel_z_change = (_vel(2) - _vel_prev(2)) / dt;
_acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change;//垂直加速度低通滤波

/* adjust limits for landing mode */
/***********************着陆处理************************/
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
//降落时限制最大倾斜和最小升力
tilt_max = _params.tilt_max_land;

if (thr_min < 0.0f) {
thr_min = 0.0f;
}

/* descend stabilized, we're landing */判断是否正在下降准备着陆
if (!_in_landing && !_lnd_reached_ground
&& (float)fabs(_acc_z_lp) < 0.1f
&& _vel_z_lp > 0.5f * _params.land_speed) {
_in_landing = true;
}

/* assume ground, cut thrust */
//判断是否已经着陆
if (_in_landing
&& _vel_z_lp < 0.1f) {
thr_max = 0.0f;
_in_landing = false;
_lnd_reached_ground = true;
}

/* once we assumed to have reached the ground always cut the thrust.
Only free fall detection below can revoke this
*/
//如果已经着陆，那么切断推力。
if (!_in_landing && _lnd_reached_ground) {
thr_max = 0.0f;
}

/* if we suddenly fall, reset landing logic and remove thrust limit */
// 如果突然下落，复位着陆的逻辑标志位并移除推力限制
if (_lnd_reached_ground
/* XXX: magic value, assuming free fall above 4m/s2 acceleration */
//假定自由落体加速度大于4米每秒的平方，速度 > 2.0f * _params.land_speed
&& (_acc_z_lp > 4.0f
|| _vel_z_lp > 2.0f * _params.land_speed)) {
thr_max = _params.thr_max;
_in_landing = false;
_lnd_reached_ground = false;
}

} else {
_in_landing = false;
_lnd_reached_ground = false;
}
/***********************着陆处理完毕************************/
/* limit min lift */
//限制最小升力
if (-thrust_sp(2) < thr_min) {
thrust_sp(2) = -thr_min;
saturation_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
/* limit max tilt */
//限制最大斜率(xy方向推力限幅)
if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
/* absolute horizontal thrust */
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();

if (thrust_sp_xy_len > 0.01f) {
/* max horizontal thrust for given vertical thrust*/
float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);

if (thrust_sp_xy_len > thrust_xy_max) {
float k = thrust_xy_max / thrust_sp_xy_len;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
}
}
}

if (_control_mode.flag_control_altitude_enabled) {
/* thrust compensation for altitude only control modes */
//推力补偿，用于高度控制
float att_comp;

if (_R(2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / _R(2, 2);

} else if (_R(2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
saturation_z = true;

} else {
att_comp = 1.0f;
saturation_z = true;
}

thrust_sp(2) *= att_comp;
}

/* limit max thrust */
//推力限幅
thrust_abs = thrust_sp.length(); /* recalculate because it might have changed */

if (thrust_abs > thr_max) {
if (thrust_sp(2) < 0.0f) {
if (-thrust_sp(2) > thr_max) {
/* thrust Z component is too large, limit it */
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
thrust_sp(2) = -thr_max;
saturation_xy = true;
saturation_z = true;

} else {
/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2));
float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
float k = thrust_xy_max / thrust_xy_abs;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}

} else {
/* Z component is negative, going down, simply limit thrust vector */
float k = thr_max / thrust_abs;
thrust_sp *= k;
saturation_xy = true;
saturation_z = true;
}

thrust_abs = thr_max;
}

/* update integrals */
//跟新推力积分
if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
}

if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;

/* protection against flipping on ground when landing */
if (thrust_int(2) > 0.0f) {
thrust_int(2) = 0.0f;
}
}

/* calculate attitude setpoint from thrust vector */
/*********************第二部分第二步的重点(3)：根据推力向量计算姿态设定值(期望姿态)***********************/
/*********************最后使用用于控制的四元数表达的旋转矩阵(旋转矩阵就是姿态)***********************/
if (_control_mode.flag_control_velocity_enabled) {
/* desired body_z axis = -normalize(thrust_vector) */
/*************先求出body_x、body_y、body_z*****************/
///////////body_x、body_y、body_z应该是方向余弦矩阵的三个列向量//////////////
math::Vector<3> body_x;
math::Vector<3> body_y;
math::Vector<3> body_z;

if (thrust_abs > SIGMA) {
body_z = -thrust_sp / thrust_abs;//body_z矩阵是推力设定值矩阵的标准化

} else {
/* no thrust, set Z axis to safe value */
body_z.zero();
body_z(2) = 1.0f;
}

/* vector of desired yaw direction in XY plane, rotated by PI/2 */
math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
//_att_sp.yaw_body = _pos_sp_triplet.current.yaw;(来自control_offboard和control_auto)
//y_C相当于是矩阵(-sin(偏航角),cos(偏航角),0)
if (fabsf(body_z(2)) > SIGMA) {
/* desired body_x axis, orthogonal to body_z */
body_x = y_C % body_z;//%是求叉积运算

/* keep nose to front while inverted upside down */
if (body_z(2) < 0.0f) {
body_x = -body_x;
}

body_x.normalize();

} else {
/* desired thrust is in XY plane, set X downside to construct correct matrix,
* but yaw component will not be used actually */
body_x.zero();
body_x(2) = 1.0f;
}

/* desired body_y axis */
body_y = body_z % body_x;
/****************再求出R<3,3>矩阵******************/
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
R(i, 0) = body_x(i);
R(i, 1) = body_y(i);
R(i, 2) = body_z(i);
}

/* copy rotation matrix to attitude setpoint topic */
/****************将R<3,3>矩阵copy到_att_sp.R_body[]******************/
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;

/* copy quaternion setpoint to attitude setpoint topic */
/****************由方向余弦旋转矩阵R得到四元数，并copy到att_sp.q_d[]/****************
math::Quaternion q_sp;
q_sp.from_dcm(R);
memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));

/* calculate euler angles, for logging only, must not be used for control */
/****************由旋转矩阵R得到姿态设置欧拉角，只是log调试用，不是给控制用的****************/
math::Vector<3> euler = R.to_euler();
_att_sp.roll_body = euler(0);
_att_sp.pitch_body = euler(1);
/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
//yaw虽然用于构建原始矩阵，但实际上旋转矩阵在奇点附近是有区别的
} else if (!_control_mode.flag_control_manual_enabled) {
/* autonomous altitude control without position control (failsafe landing),
* force level attitude, don't change yaw */
//没有位置控制的高度控制(故障安全降落)，固定水平姿态，不改变yaw角
R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);

/* copy rotation matrix to attitude setpoint topic */
//将旋转矩阵R<3,3>  copy到_att_sp.R_body[]
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;

/* copy quaternion setpoint to attitude setpoint topic */
//由方向余弦旋转矩阵R得到四元数，并copy到_att_sp.q_d[]
math::Quaternion q_sp;
q_sp.from_dcm(R);
memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));

_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
}

_att_sp.thrust = thrust_abs;
//推力向量的长度赋值给姿态推力设定值(att_sp.thrust)，这样才够各个方向力度分配

/* save thrust setpoint for logging */
//用于log，方便调试
_local_pos_sp.acc_x = thrust_sp(0) * ONE_G;
_local_pos_sp.acc_y = thrust_sp(1) * ONE_G;
_local_pos_sp.acc_z = thrust_sp(2) * ONE_G;

_att_sp.timestamp = hrt_absolute_time();//测出用的绝对时间

} else {
reset_int_z = true;
}
}
/*********************第三部分：将之前程序得到的各种信息填充_local_pos_sp结构体，并发布出去***********************/
/* fill local position, velocity and thrust setpoint */
_local_pos_sp.timestamp = hrt_absolute_time();
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
//第二部分第一步：产生位置/速度设定值(期望值)
_local_pos_sp.yaw = _att_sp.yaw_body;
_local_pos_sp.vx = _vel_sp(0);
_local_pos_sp.vy = _vel_sp(1);
_local_pos_sp.vz = _vel_sp(2);
//第二部分第二步的重点(1):产生可利用的速度设定值(期望值)
/* publish local position setpoint */
if (_local_pos_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);

} else {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
}

}
////////高度控制、位置控制、爬升速率控制、速度控制的相关程序结束//////////
//////////////其他情况(位置控制失能)就复位各种设定值////////////
else {
/* position controller disabled, reset setpoints */
_reset_alt_sp = true;
_reset_pos_sp = true;
_mode_auto = false;
reset_int_z = true;
reset_int_xy = true;
control_vel_enabled_prev = false;

/* store last velocity in case a mode switch to position control occurs */
_vel_sp_prev = _vel;
}
//////////////以下判断是并列于“高度控制、位置控制、爬升速率控制、速度控制”的判断////////////
//////////////所以会出现混控现象，在执行任务的时候还可以遥控控制/////////////////
//////////////手动控制和姿态控制都使能，则运行以下程序产生姿态设定值////////////////
/* generate attitude setpoint from manual controls */
if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {

/* reset yaw setpoint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _yaw;
}

/* do not move yaw while sitting on the ground */
//当飞行器在地上是，不要移动yaw
else if (!_vehicle_status.condition_landed &&
!(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {

/* we want to know the real constraint, and global overrides manual */
const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max :
_params.global_yaw_max;
const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;

_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;//旋转*旋转比率
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);//期望yaw
float yaw_offs = _wrap_pi(yaw_target - _yaw);//期望yaw-飞机yaw=需要调整的yaw
//_wrap_pi()函数是将输入角度参数控制到(0,2*pi)

// If the yaw offset became too big for the system to track stop
// shifting it

// XXX this needs inspection - probably requires a clamp, not an if
if (fabsf(yaw_offs) < yaw_offset_max) {
_att_sp.yaw_body = yaw_target;
}
}

/* control roll and pitch directly if we no aiding velocity controller is active */
if (!_control_mode.flag_control_velocity_enabled) {
_att_sp.roll_body = _manual.y * _params.man_roll_max;
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
}

/* control throttle directly if no climb rate controller is active */
//如果没有爬升速率控制器，则直接推力控制
if (!_control_mode.flag_control_climb_rate_enabled) {
float thr_val = throttle_curve(_manual.z, _params.thr_hover);//手动推力的转换，以便控制器输出控制推力力度
_att_sp.thrust = math::min(thr_val, _manual_thr_max.get());

/* enforce minimum throttle if not landed */
//如果没有着陆，则需要限制最小推力
if (!_vehicle_status.condition_landed) {
_att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
}
}

math::Matrix<3, 3> R_sp;

/* construct attitude setpoint rotation matrix */
//构建姿态设定值的旋转矩阵并copy到_att_sp.R_body[]
R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));

/* reset the acceleration set point for all non-attitude flight modes */
//非姿态飞行模式，复位加速度设定值
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {

_thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust);
}

/* copy quaternion setpoint to attitude setpoint topic */
//将姿态设定值的旋转矩阵转换成四元数矩阵并copy到_att_sp.q_d[]，以便发布
math::Quaternion q_sp;
q_sp.from_dcm(R_sp);
memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
_att_sp.timestamp = hrt_absolute_time();

}
///////////////手动控制部分结束///////////////////////
///////////////手动控制失能///////////////
else {
reset_yaw_sp = true;
}
/////////////跟新前一个时刻的速度用于D部分(D应该是PID的D)////////////////////
/* update previous velocity for velocity controller D part */
_vel_prev = _vel;
/////////////////发布姿态设定值///////////////////
/////////////////如果位置/速度失能而机外(offboard)使能，则不发布姿态设定值//////////////
/////////////////因为这种情况姿态设定值是通过mavlink应用发布的//////////////
/////////////////飞机工作于垂直起降或者做一个过渡，也不发布，因为此时由垂直起降控制部分发布/////////
/* publish attitude setpoint
* Do not publish if offboard is enabled but position/velocity control is disabled,
* in this case the attitude setpoint is published by the mavlink app. Also do not publish
* if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate
* attitude setpoints for the transition).
*/
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {

if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);

} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
}
///////////////////手动控制后复位高度控制的积分(悬停油门)，以便更好的转变为手动模式////////////////
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled
&& !_control_mode.flag_control_climb_rate_enabled;
}

mavlink_log_info(_mavlink_fd, "[mpc] stopped");

_control_task = -1;
}

control_manual()函数

void
MulticopterPositionControl::control_manual(float dt)
{
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 */
/* 将自稳模式的油门杆转换成控制垂直速度设定值(以中间速度为0，往上拨速度向上，往下拨速度向下，速度大小与拨动幅度成正比) */
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 */
/* 水平速度设定值由roll和pitch杆确定 */
req_vel_sp(0) = _manual.x;
req_vel_sp(1) = _manual.y;
}

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);//由欧拉角得到旋转矩阵
/**Firmware/src/lib/mathlib/math/Matrix.hpp
* create a rotation matrix from given euler angles
* based on http://gentlenav.googlecode.com/files/EulerAngles.pdf *
*	void from_euler(float roll, float pitch, float yaw) {
*		float cp = cosf(pitch);
*		float sp = sinf(pitch);
*		float sr = sinf(roll);
*		float cr = cosf(roll);
*		float sy = sinf(yaw);
*		float cy = cosf(yaw);
*
*		data[0][0] = cp * cy;
*		data[0][1] = (sr * sp * cy) - (cr * sy);
*		data[0][2] = (cr * sp * cy) + (sr * sy);
*		data[1][0] = cp * sy;
*		data[1][1] = (sr * sp * sy) + (cr * cy);
*		data[1][2] = (cr * sp * sy) - (sr * cy);
*		data[2][0] = -sp;
*		data[2][1] = sr * cp;
*		data[2][2] = cr * cp;
*	}
*/
math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(
_params.vel_max); // in NED and scaled to actual velocity
// 在NED坐标系下，还原到真实的速度
/////////////以下为水平轴辅助速度模式：用户控制速度，但是如果速度很小，那么相应轴上的位置保持不变/////////////////
/*
* 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) {
if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy
&& fabsf(_vel(1)) < _params.hold_max_xy)) {
_pos_hold_engaged = true;

} 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);
}
}
////////////以下为垂直轴辅助速度模式：用户控制速度，但是如果速度很小，那么相应轴上的位置保持不变/////////////////
/* 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) {
_alt_hold_engaged = true;

} else {
_alt_hold_engaged = false;
}
}

} else {
_alt_hold_engaged = false;
}
////////////以上为垂直轴辅助速度模式：用户控制速度，但是如果速度很小，那么相应轴上的位置保持不变/////////////////
/* 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);
_pos_sp(2) = _pos(2);
}
}
}

control_offboard()函数

void
MulticopterPositionControl::control_offboard(float dt)
{
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);

if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
//////////水平轴设定//////////////
if (_pos_sp_triplet.current.valid) {
if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
//控制模式-位置控制使能&&当前位置设定值合法，那么进行位置控制
/* control position */
_pos_sp(0) = _pos_sp_triplet.current.x;
_pos_sp(1) = _pos_sp_triplet.current.y;

} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
//控制模式-速度控制使能&&当前速度设定值合法
/* control velocity */
/* reset position setpoint to current position if needed */
reset_pos_sp();//速度控制时，需要复位位置

/* set position setpoint move rate */
_vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy;

_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
}
////////////yaw姿态设定///////////
if (_pos_sp_triplet.current.yaw_valid) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;

} else if (_pos_sp_triplet.current.yawspeed_valid) {
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
}
/////////////垂直轴设定///////////
if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) {
//控制模式：高度控制模式&&当前位置设定是否合法
/* Control altitude */
_pos_sp(2) = _pos_sp_triplet.current.z;

} else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) {
//控制模式：爬升速度控制模式&&当前速度设定是否合法
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();

/* set altitude setpoint move rate */
_vel_sp(2) = _pos_sp_triplet.current.vz;

_run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */
}

} else {
reset_pos_sp();
reset_alt_sp();
}
}

control_auto()函数

void MulticopterPositionControl::control_auto(float dt)
{
///////////////复位位置设定值在自动模式下或者我们没有在动作控制模式(MC mode)下////////////
/* reset position setpoint on AUTO mode activation or if we are not in MC mode */
if (!_mode_auto || !_vehicle_status.is_rotary_wing) {
if (!_mode_auto) {
_mode_auto = true;
}

_reset_pos_sp = true;
_reset_alt_sp = true;

reset_pos_sp();
reset_alt_sp();
}
//////////////////获取三重位置设定值//////////////////
//Poll position setpoint
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);

if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
///////////////当前三重位置设定值是否为有限数，如果是则为有效值//////////////
//Make sure that the position setpoint is valid
if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) ||
!PX4_ISFINITE(_pos_sp_triplet.current.lon) ||
!PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
_pos_sp_triplet.current.valid = false;
}
}
/////////////初始化标志位///////////////
bool current_setpoint_valid = false;
bool previous_setpoint_valid = false;

math::Vector<3> prev_sp;
math::Vector<3> curr_sp;
//////////////如果当前三重位置设定值合法,将当前设定值经纬度和高度转换成地坐标系的xyz值////////////
if (_pos_sp_triplet.current.valid) {

/* project setpoint to local frame */
/*这个函数在此cpp里面经常使用，是将将经纬度转换成地坐标系xy值*/
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&curr_sp.data[0], &curr_sp.data[1]);
curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);

if (PX4_ISFINITE(curr_sp(0)) &&
PX4_ISFINITE(curr_sp(1)) &&
PX4_ISFINITE(curr_sp(2))) {
current_setpoint_valid = true;
}
}
//////////////如果上一时刻三重位置设定值合法,将当前设定值经纬度和高度转换成地坐标系的xyz值////////////
if (_pos_sp_triplet.previous.valid) {
map_projection_project(&_ref_pos,
_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
&prev_sp.data[0], &prev_sp.data[1]);
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);

if (PX4_ISFINITE(prev_sp(0)) &&
PX4_ISFINITE(prev_sp(1)) &&
PX4_ISFINITE(prev_sp(2))) {
previous_setpoint_valid = true;
}
}
///////////////如果当前位置设定值合法/////////////////
if (current_setpoint_valid) {
/*********************	下部分只是将设定值进行比例变换，缩小进一个区间   ******************/
/////////////范围区间：位置误差导致的最大允许速度 为1，也就是说(0,1)之间///////////////
/* scaled space: 1 == position error resulting max allowed speed */
math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here
/*用_params.pos_p的每一个元素除以_params.vel_max对应位置的每一个元素，结果赋值给scale
*	const Vector<N> edivide(const Vector<N> &v) const {
*		Vector<N> res;
*
*		for (unsigned int i = 0; i < N; i++)
*			res.data[i] = data[i] / v.data[i];
*
*		return res;
*	}
*/
////////////将当前设定值转换到范围区间中//////////////////
/* convert current setpoint to scaled space */
math::Vector<3> curr_sp_s = curr_sp.emult(scale);
/*	用curr_sp的每一个元素和scale对应位置的每一个元素相乘，结果赋值给curr_sp_s
*	Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const
*	{
*		Matrix<Type, M, N> res;
*		const Matrix<Type, M, N> &self = *this;
*
*		for (size_t i = 0; i < M; i++) {
*			for (size_t j = 0; j < N; j++) {
*				res(i , j) = self(i, j)*other(i, j);
*			}
*		}
*
*		return res;
*	}
*/
/*********************	上部分只是将设定值进行比例变换，缩小进一个区间   ******************/
///////////////////默认使用的当前设定值///////////////////////////
/* by default use current setpoint as is */
math::Vector<3> pos_sp_s = curr_sp_s;
//////////////////判断当前类型是否是位置设定类型&&上一次设定值是否合法////////////////////
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && previous_setpoint_valid) {
/* follow "previous - current" line */
//////////////////遵守"previous - current"主线///////////////////
if ((curr_sp - prev_sp).length() > MIN_DIST) {

/* find X - cross point of unit sphere and trajectory */
math::Vector<3> pos_s = _pos.emult(scale);//copy的_local_pos位置信息 * 比例
math::Vector<3> prev_sp_s = prev_sp.emult(scale);//带了_s的都是乘以了比例的 scale
math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
float curr_pos_s_len = curr_pos_s.length();
/////////////////根据飞行器位置距离当前位置设定点的距离分2种情况：小于单位半径和不小于单位半径////////////////////
////////////////////小于单位半径////////////////////
if (curr_pos_s_len < 1.0f) {
/* copter is closer to waypoint than unit radius */
/* check next waypoint and use it to avoid slowing down when passing via waypoint */
/*飞行器距离航点在单位半径以内*/
/*在奔向当前航点的时候检测下一个航点，避免通过当前航点时减速*/
if (_pos_sp_triplet.next.valid) {
math::Vector<3> next_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
&next_sp.data[0], &next_sp.data[1]);
next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt);

if ((next_sp - curr_sp).length() > MIN_DIST) {
math::Vector<3> next_sp_s = next_sp.emult(scale);

/* calculate angle prev - curr - next */
math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
/*标准化prev_curr_s
* returns the normalized version of this vector
*
*	Vector<N> normalized() const {
*		return *this / length();
*	}
*/

/* cos(a) * curr_next, a = angle between current and next trajectory segments */
/* prev_curr_s_norm是单位向量(当前位置设定-前一次位置设定)，curr_next_s是另一个向量(下一次位置设定-当前位置设定)*/
* 向量相乘点乘 ab=丨a丨丨b丨cosα，结果是一代数
* 向量相乘叉乘|向量c|=|向量a×向量b|=|a||b|sin，结果是一向量
*/
float cos_a_curr_next = prev_curr_s_norm * curr_next_s;

/* cos(b), b = angle pos - curr_sp - prev_sp */
/* curr_pos_s是向量当前位置指向实际位置(实际位置-当前位置设定)
* prev_curr_s_norm是前一次位置设定指向当前位置设定的单位向量(当前位置设定-前一次位置设定)
* curr_pos_s_len是当前位置设定与实际位置之间长度
* 所以cos_b就是pos - curr_sp - prev_sp三点连线的角度的余弦值
*/
float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;

if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {//a、b为锐角
float curr_next_s_len = curr_next_s.length();

/* if curr - next distance is larger than unit radius, limit it */
/*当前位置设定到下个位置设定的距离超过单位半径*/
if (curr_next_s_len > 1.0f) {
cos_a_curr_next /= curr_next_s_len;//cos_a_curr_next=cos(a) * curr_next/||curr_next||
}

/* feed forward position setpoint offset */
/*前馈位置设定值偏移*/
math::Vector<3> pos_ff = prev_curr_s_norm *
cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
pos_sp_s += pos_ff;
}
}
}

}
////////////////////不小于单位半径////////////////////
else {
bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);

if (near) {
/* unit sphere crosses trajectory */单位球越过轨迹

} else {
/* copter is too far from trajectory */
/* if copter is behind prev waypoint, go directly to prev waypoint */
/* 	飞行器离设定轨迹很远
*	如果飞行器在前一个设定位置后面，则先到前一个设定位置
*	角pos_sp - prev_sp - curr_sp大于90°
*/
if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) {
pos_sp_s = prev_sp_s;
}

/* if copter is in front of curr waypoint, go directly to curr waypoint */
/* 如果飞行器在前一个设定位置前面，则到当前设定位置*/
if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
pos_sp_s = curr_sp_s;
}

pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
}
}
}
}
/* 由上述程序大概就可以看出控制逻辑
* 先根据任务设定前一个、当前、下一个位置标定(prev_sp_s、curr_sp_s、next_sp_s)，用于控制飞行器按照此轨迹飞行
* pos_sp_s是实时位置设定值，用于指导飞行器具体如何一个点一个点的靠近轨迹标定值(prev_sp_s、curr_sp_s、next_sp_s)
* pos_s是飞行器实时位置
* 带了_s的都是经过大小比例缩放的，不是实际值
*/
////////////////以下部分是限速用的//////////////
/* move setpoint not faster than max allowed speed */
math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);

/* difference between current and desired position setpoints, 1 = max speed */
math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
float d_pos_m_len = d_pos_m.length();

if (d_pos_m_len > dt) {
pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
}

/* scale result back to normal space */
_pos_sp = pos_sp_s.edivide(scale);
////////////////以上部分是限速用的//////////////
/* update yaw setpoint if needed */
/* 跟新yaw的姿态设定值 */
if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
/////////////以下部分是用于起飞到位置巡航光滑切换///////////////////
/*
* if we're already near the current takeoff setpoint don't reset in case we switch back to posctl.
* this makes the takeoff finish smoothly.
*/
if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
|| _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)
&& _pos_sp_triplet.current.acceptance_radius > 0.0f
/* need to detect we're close a bit before the navigator switches from takeoff to next waypoint */
&& (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.2f) {
_reset_pos_sp = false;
_reset_alt_sp = false;

/* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */
/* 在中断任务的情况下，不要去航点，但留在当前位置 */
} else {
_reset_pos_sp = true;
_reset_alt_sp = true;
}
/////////////以上部分是用于起飞到位置巡航光滑切换///////////////////
} else {
/* no waypoint, do nothing, setpoint was already reset */
}
}
以上计算都是基于map_projection_project(&_ref_pos,sp.lat, sp.lon,&sp.data[0], &sp.data[1]);函数的计算(将经纬度转换成地坐标系xy值)也就是说是基于GPS的位置自动控制

map_projection_project()函数

map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&curr_sp.data[0], &curr_sp.data[1]);

输入参数：	&_ref_pos
/* lat/lon are in radians */
struct map_projection_reference_s {//地图投影参考
double lat_rad;
double lon_rad;
double sin_lat;
double cos_lat;
bool init_done;
uint64_t timestamp;
};
纬度是地心到某地连线与地心到赤道连线的夹角就是某地纬度，赤道为0度，两极是90度
经度是英国伦敦附近的格林尼治天文台为0度，向东西两边逐加分别为东经和西经，到背面就是180度
度数/360=弧度/2π
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon
position_setpoint_s结构体中double lat;(纬度)double lon;(经度)，由navigator发布
&curr_sp.data[0], &curr_sp.data[1]
x,y方向位置

__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,
float *y)
输入参数：	*ref
/* lat/lon are in radians */
struct map_projection_reference_s {//地图投影参考
double lat_rad;
double lon_rad;
double sin_lat;
double cos_lat;
bool init_done;
uint64_t timestamp;
};
纬度是地心到某地连线与地心到赤道连线的夹角就是某地纬度，赤道为0度，两极是90度
经度是英国伦敦附近的格林尼治天文台为0度，向东西两边逐加分别为东经和西经，到背面就是180度
度数/360=弧度/2π
double lat, double lon,
double lat;(纬度)double lon;(经度)
float *x, float *y
x,y方向位置
实现的功能：将经纬度转换成地坐标系xy值
{
if (!map_projection_initialized(ref)) {
return -1;
}

double lat_rad = lat * M_DEG_TO_RAD;//#define M_DEG_TO_RAD 	0.01745329251994角度转弧度
double lon_rad = lon * M_DEG_TO_RAD;//#define M_RAD_TO_DEG 	57.2957795130823弧度转角度

double sin_lat = sin(lat_rad);
double cos_lat = cos(lat_rad);
double cos_d_lon = cos(lon_rad - ref->lon_rad);
//ref->lon_rad是copy ORB_ID(vehicle_local_position)主题，经过update_ref()里面的
//map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
//ref->lon_rad = lon_0 * M_DEG_TO_RAD;

double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));

*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
*y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
//#define CONSTANTS_RADIUS_OF_EARTH(地球半径)			6371000			/* meters (m)		*/
return 0;
}

常用矩阵向量函数

/**Firmware/src/lib/mathlib/math/Quaternion.hpp
* create rotation matrix for the quaternion
*/由四元数得到方向余弦旋转矩阵
Matrix<3, 3> to_dcm(void) const {
Matrix<3, 3> R;
float aSq = data[0] * data[0];
float bSq = data[1] * data[1];
float cSq = data[2] * data[2];
float dSq = data[3] * data[3];
R.data[0][0] = aSq + bSq - cSq - dSq;
R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]);
R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]);
R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]);
R.data[1][1] = aSq - bSq + cSq - dSq;
R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]);
R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]);
R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]);
R.data[2][2] = aSq - bSq - cSq + dSq;
return R;
}

/**Firmware/src/lib/mathlib/math/Quaternion.hpp
* set quaternion to rotation by DCM
* Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf */由方向余弦旋转矩阵得到四元数
void from_dcm(const Matrix<3, 3> &dcm) {
float tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2];
if (tr > 0.0f) {
float s = sqrtf(tr + 1.0f);
data[0] = s * 0.5f;
s = 0.5f / s;
data[1] = (dcm.data[2][1] - dcm.data[1][2]) * s;
data[2] = (dcm.data[0][2] - dcm.data[2][0]) * s;
data[3] = (dcm.data[1][0] - dcm.data[0][1]) * s;
} else {
/* Find maximum diagonal element in dcm
* store index in dcm_i */
int dcm_i = 0;
for (int i = 1; i < 3; i++) {
if (dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) {
dcm_i = i;
}
}
int dcm_j = (dcm_i + 1) % 3;
int dcm_k = (dcm_i + 2) % 3;
float s = sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] -
dcm.data[dcm_k][dcm_k]) + 1.0f);
data[dcm_i + 1] = s * 0.5f;
s = 0.5f / s;
data[dcm_j + 1] = (dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s;
data[dcm_k + 1] = (dcm.data[dcm_k][dcm_i] + dcm.data[dcm_i][dcm_k]) * s;
data[0] = (dcm.data[dcm_k][dcm_j] - dcm.data[dcm_j][dcm_k]) * s;
}
}

/**Firmware/src/lib/mathlib/math/Matrix.hpp
* get euler angles from rotation matrix
*/由旋转矩阵得到欧拉角
Vector<3> to_euler(void) const {
Vector<3> euler;
euler.data[1] = asinf(-data[2][0]);

if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) {
euler.data[0] = 0.0f;
euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0];

} else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) {
euler.data[0] = 0.0f;
euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0];

} else {
euler.data[0] = atan2f(data[2][1], data[2][2]);
euler.data[2] = atan2f(data[1][0], data[0][0]);
}

return euler;
}

/**Firmware/src/lib/mathlib/math/Matrix.hpp
* set zero matrix  零矩阵
*/
void zero(void) {
memset(data, 0, sizeof(data));
}

/**Firmware/src/lib/mathlib/math/Matrix.hpp
* set identity matrix单位矩阵
*/
void identity(void) {
memset(data, 0, sizeof(data));
unsigned int n = (M < N) ? M : N;

for (unsigned int i = 0; i < n; i++)
data[i][i] = 1;
}

typedef struct pollfd px4_pollfd_struct_t;

/* In the standard poll() definition, the size of the event set is 'short'.
* Here we pick the smallest storage element that will contain all of the
* poll events.
*/
/* 在标准轮询（）的定义，设置事件的大小是“short”。 在这里，最小存储元件将包含所有的轮训事件*/
typedef uint8_t pollevent_t;

/* This is the Nuttx variant of the standard pollfd structure. */
/*这是标准的pollfd结构的Nuttx变量*/
struct pollfd
{
int         fd;       /* The descriptor being polled */
sem_t      *sem;      /* Pointer to semaphore used to post output event */
pollevent_t events;   /* The input event flags */
pollevent_t revents;  /* The output event flags */
FAR void   *priv;     /* For use by drivers */
};

/**Firmware/src/lib/mathlib/math/Vector.hpp
* element by element multiplication
*/
Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const
{
Matrix<Type, M, N> res;
const Matrix<Type, M, N> &self = *this;

for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j)*other(i, j);
}
}

return res;
}

/**Firmware/src/lib/mathlib/math/Vector.hpp
* element by element division
*/
const Vector<N> edivide(const Vector<N> &v) const {
Vector<N> res;

for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] / v.data[i];

return res;
}

/**Firmware/src/lib/mathlib/math/Matrix.hpp
* create a rotation matrix from given euler angles
* based on http://gentlenav.googlecode.com/files/EulerAngles.pdf */由欧拉角得到旋转矩阵
void from_euler(float roll, float pitch, float yaw) {
float cp = cosf(pitch);
float sp = sinf(pitch);
float sr = sinf(roll);
float cr = cosf(roll);
float sy = sinf(yaw);
float cy = cosf(yaw);

data[0][0] = cp * cy;
data[0][1] = (sr * sp * cy) - (cr * sy);
data[0][2] = (cr * sp * cy) + (sr * sy);
data[1][0] = cp * sy;
data[1][1] = (sr * sp * sy) + (cr * cy);
data[1][2] = (cr * sp * sy) - (sr * cy);
data[2][0] = -sp;
data[2][1] = sr * cp;
data[2][2] = cr * cp;
}

如果您觉得此文对您的发展有用，请随意打赏。 
您的鼓励将是笔者书写高质量文章的最大动力^_^!!