- 首先看bool PositionControl::update(const float dt)函数，该函数先进行有效性判断。
- 跳转到_positionControl函数
if (valid) {
_positionControl();
- 分析_positionControl函数
3.1 通过P控制实现速度调节
Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p);
- _pos_sp - _pos，设定位置点-当前位置点
3.2 分析.emult(_gain_pos_p)
Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const
- 返回类型：Matrix<Type, M, N>。这表明函数返回一个新的矩阵，该矩阵与原始矩阵（调用emult的矩阵）具有相同的类型、行数和列数。
- 函数名：emult。这是一个自定义的函数名，用于表示元素级乘法操作。在Eigen库中，元素级乘法通常使用cwiseProduct或简单的*操作符（如果另一个操作数也是矩阵）来实现，但emult可能是为了特定的项目或库而定义的别名或封装。
- 参数：const Matrix<Type, M, N> &other。这是一个对另一个Matrix<Type, M, N>类型的常量引用的参数。这意味着emult函数可以接受与调用它的矩阵相同大小和类型的另一个矩阵作为输入，并且不会修改这个输入矩阵。
3.3 分析_gain_pos_p
- 比例系数，可能是在路径"src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp"
3.4 分析ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position);
- 这个函数直接修改了_vel_sp向量（或可能是它的一个别名或引用），因此在调用此函数后，_vel_sp的值将反映添加vel_sp_position（忽略NaN元素）后的结果
- 作用为：将位置误差乘比例系数后，加入前馈速度_vel_sp，并赋值给_vel_sp
3.5 分析ControlMath::setZeroIfNanVector3f(vel_sp_position);
- 确保vel_sp_position向量中的所有值都是有效的，不会因为包含NaN值而导致后续的计算或控制逻辑出现问题。通过将NaN值替换为0，函数提供了一种简单的方法来“清理”可能由于传感器故障、计算错误或其他原因而引入的无效数据
3.6 分析：
_vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal);
- 为方便描述，另vel_sp_position.xy()=v0，(_vel_sp - vel_sp_position).xy()=v1
- v0+v1=_vel_sp，也就是期望的速度，该值应小于_lim_vel_horizontal
- v0比v1更重要，因为这可以较少误差。通过分类讨论，赋值给_vel_sp.xy()，也就是新的期望速度的xy合速度（猜测）
_vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down);
- 限制z轴速度，需要注意，标号从0开始
- 回到update函数，并且跳转到_velocityControl(dt);
- 分析_velocityControl(dt);
5.1 公式：
5.2 代码解读
// PID velocity control
Vector3f vel_error = _vel_sp - _vel;
Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);
// No control input from setpoints or corresponding states which are NAN
ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);
- 跳转到_accelerationControl()函数
6.1 计算推力方向
- 公式：
- 代码：
Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();
6.2 倾角限制
- 将b限制到b'
- 代码
ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);
6.3 设置z方向加速度的需求油门
- 公式：
- 代码：
// Scale thrust assuming hover thrust produces standard gravity
float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
// Project thrust to planned body attitude
collective_thrust /= (Vector3f(0, 0, 1).dot(body_z));
6.4 限制油门大小，油门量是负号？哪个小选哪个避免z轴方向油门量不够
collective_thrust = math::min(collective_thrust, -_lim_thr_min);
6.5 大小乘方向，获得期望油门拉力方向
_thr_sp = body_z * collective_thrust;
5.3 跳回_velocityControl函数
- 代码：竖直方向抗饱和积分
// Integrator anti-windup in vertical direction
if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) ||
(_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.0f)) {
vel_error(2) = 0.f;
}
- 解读：如果上述任一条件为真，则执行vel_error(2) = 0.f;，即将垂直方向上的速度误差重置为0。这样做的目的是防止积分器继续累积误差，当系统已经达到其物理限制时，这样做可以避免积分器饱和，从而帮助系统更快地响应变化或恢复稳定性。
5.4 油门量分配，优先满足竖直方向 - 代码：
const Vector2f thrust_sp_xy(_thr_sp);
- 解读：创建了一个Vector2f类型的常量对象thrust_sp_xy，使用_thr_sp作为构造函数的参数来初始化thrust_sp_xy。这里有一个隐含的假设，即Vector2f类有一个可以接受单个浮点数作为参数的构造函数。随后进行油门量分配。
5.5 水平方向上的跟踪抗饱和算法以及整体的抗饱和积分，用来5.2中的速度还误差抗饱和积分
// Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output
// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
const Vector2f acc_sp_xy_limited = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);
const float arw_gain = 2.f / _gain_vel_p(0);
vel_error.xy() = Vector2f(vel_error) - (arw_gain * (Vector2f(_acc_sp) - acc_sp_xy_limited));
// Make sure integral doesn't get NAN
ControlMath::setZeroIfNanVector3f(vel_error);
// Update integral part of velocity control
_vel_int += vel_error.emult(_gain_vel_i) * dt;
// limit thrust integral
_vel_int(2) = math::min(fabsf(_vel_int(2)), CONSTANTS_ONE_G) * sign(_vel_int(2));