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 statewas_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 controlledif (!_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 abovethrust_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 *///当飞行器在地上是，不要移动yawelse 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);//期望yawfloat 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 ifif (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 rangereq_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 setpointbool 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 validif (!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的都是乘以了比例的 scalemath::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.lonposition_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 *yx,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;}

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

pixhawk mc_pos_control.cpp源码解读相关推荐

PyTorch 源码解读之即时编译篇
点击上方"AI遇见机器学习",选择"星标"公众号重磅干货,第一时间送达作者丨OpenMMLab 来源丨https://zhuanlan.zhihu.com/ ...
Ubuntu 16.04下Caffe-SSD的应用（四）——ssd_pascal.py源码解读
前言 caffe-ssd所有的训练时的参数,全部由ssd_pascal.py来定义,之后再去调用相关的脚本和函数,所以想要训练自己的数据,首先要明白ssd_pascal.py各个定义参数的大体意思. ...
MFC源码解读（一）最原始一个MFC程序，手写不用向导
从这一篇开始,详细记录一下MFC的源码解读四个文件,分别为: stdafx.h,stdafx.cpp,hello.h,hello.cpp 代码如下: //stdafx.h #include < ...
PostgreSQL 源码解读（203）- 查询#116(类型转换实现)
本节简单介绍了PostgreSQL中的类型转换的具体实现. 解析表达式,涉及不同数据类型时: 1.如有相应类型的Operator定义(pg_operator),则尝试进行类型转换,否则报错; 2.如有 ...
hdl_graph_slam源码解读(四)：关键帧
目录 1. 整体介绍 2. 详细介绍 2.1 KeyFrame类 2.2 KeyFrameSnapshot类 2.3 KeyFrameUpdater类参考文献 1. 整体介绍在图优化中,关键帧的作 ...
PyTorch 源码解读之 cpp_extension：讲解 C++/CUDA 算子实现和调用全流程
"Python 用户友好却运行效率低","C++ 运行效率较高,但实现一个功能代码量会远大于 Python".平常学习工作中你是否常听到类似的说法?在 Pyth ...
Bert系列（二）——源码解读之模型主体
本篇文章主要是解读模型主体代码modeling.py.在阅读这篇文章之前希望读者们对bert的相关理论有一定的了解,尤其是transformer的结构原理,网上的资料很多,本文内容对原理部分就不做过多 ...
Bert系列（三）——源码解读之Pre-train
https://www.jianshu.com/p/22e462f01d8c pre-train是迁移学习的基础,虽然Google已经发布了各种预训练好的模型,而且因为资源消耗巨大,自己再预训练也不现 ...
linux下free源码,linux命令free源码解读：Procps free.c
linux命令free源码解读 linux命令free源码解读:Procps free.c 作者:isayme 发布时间:September 26, 2011 分类:Linux 我们讨论的是linux ...

pixhawk mc_pos_control.cpp源码解读

pixhawk mc_pos_control.cpp源码解读相关推荐

最新文章

热门文章