一、mc_pos_control位置控制代码功能介绍
PX4飞控代码支持多种不同的机型，包含固定翼、多旋翼、还有船和车。其中本次分析的mc_pos_control是针对多旋翼这种的机型的位置控制功能。其他的机型如固定翼参见fw_postion_control等。
首先说下位置控制的作用。在前面navigator章节中，根据不同的任务类型，产生不同的期望位置点。为了达到这些位置，需要对无人机进行的螺旋桨进行控制。螺旋桨的转速只会改变无人机俯仰或者横滚的角速度，所以需要计算为了到达期望的位置，无人机需要的速度。无人机的速度又是通过倾斜角度来达到的，因此就关联出速度和角度的关系。角度的控制又是通过角速度的积分来控制的，这就是采用串级PID的原因。结合PID的算法原理，得出下图：
整个mc_pos_control也是以上图为框架，在此基础上为了飞行的平稳等，增加一些约束的。
二、mc_pos_control功能结构图
三、代码分析
MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
SuperBlock(nullptr, "MPC"),
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
// 根据vtol来决定发布主题
// mc_virtual_attitude_setpoint：是多旋翼无人机特有的控制器；
// vehicle_attitude_setpoint：是通用的姿态控制器，适用于各种类型的无人机。
_vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
// 速度导数滤波器对象
_vel_x_deriv(this, "VELD"),
_vel_y_deriv(this, "VELD"),
_vel_z_deriv(this, "VELD")
{
// 参数更新
parameters_update(true);
/* 设置飞行器在执行某些任务时的倾斜角度的限制。setSlewRate函数用于设置倾斜角度的限制斜率，
.2f是斜率的值，表示飞行器的倾斜角度变化速率限制为每秒0.2弧度。
这里的斜率是指限制器允许的变化速率，
例如，在每个时间间隔内限制器可以增加的最大值，也称为限制器的斜率。
*/
_tilt_limit_slew_rate.setSlewRate(.2f);
//订阅起飞状态
_takeoff_status_pub.advertise();
}
void MulticopterPositionControl::Run()
{
// 检查是否需要退出程序
if (should_exit()) {
_local_pos_sub.unregisterCallback();// 取消本地位置订阅的回调函数
exit_and_cleanup();// 执行清理函数
return;
}
// reschedule backup
ScheduleDelayed(100_ms);
//非强制 更新参数
parameters_update(false);
// 启动计时
perf_begin(_cycle_perf);
vehicle_local_position_s vehicle_local_position;
// 等待并更新 飞行器当前的本地位置信息
if (_local_pos_sub.update(&vehicle_local_position)) {
// 计算两次循环之间的时间差（以微秒为单位）。* 1e-6f 将时间差转换为秒。
// 确保时间差的值在 0.002 秒和 0.04 秒之间
const float dt =
math::constrain(((vehicle_local_position.timestamp_sample - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f);
_time_stamp_last_loop = vehicle_local_position.timestamp_sample;
// set _dt in controllib Block for BlockDerivative
setDt(dt);
// 更新控制模式
if (_vehicle_control_mode_sub.updated()) {
const bool previous_position_control_enabled = _vehicle_control_mode.flag_multicopter_position_control_enabled;
if (_vehicle_control_mode_sub.update(&_vehicle_control_mode)) {
if (!previous_position_control_enabled && _vehicle_control_mode.flag_multicopter_position_control_enabled) {
_time_position_control_enabled = _vehicle_control_mode.timestamp;
} else if (previous_position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled) {
// clear existing setpoint when controller is no longer active
_setpoint = PositionControl::empty_trajectory_setpoint;
}
}
}
// 更新飞行器当前的着陆检测状态信息
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
if (_param_mpc_use_hte.get()) {
hover_thrust_estimate_s hte;
// 更新飞行器当前的悬停推力估计信息
if (_hover_thrust_estimate_sub.update(&hte)) {
if (hte.valid) {
_control.updateHoverThrust(hte.hover_thrust);
}
}
}
//将飞行器当前的轨迹跟踪目标点信息保存到_setpoint结构体（存储本地位置控制器的目标位置和速度等信息）
// 后面是要发布出去的目标点数据
_trajectory_setpoint_sub.update(&_setpoint);
// adjust existing (or older) setpoint with any EKF reset deltas
if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) {
if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
_setpoint.velocity[0] += vehicle_local_position.delta_vxy[0];
_setpoint.velocity[1] += vehicle_local_position.delta_vxy[1];
}
if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
_setpoint.velocity[2] += vehicle_local_position.delta_vz;
}
if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) {
_setpoint.position[0] += vehicle_local_position.delta_xy[0];
_setpoint.position[1] += vehicle_local_position.delta_xy[1];
}
if (vehicle_local_position.z_reset_counter != _z_reset_counter) {
_setpoint.position[2] += vehicle_local_position.delta_z;
}
if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
_setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading);
}
}
if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
_vel_x_deriv.reset();
_vel_y_deriv.reset();
}
if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
_vel_z_deriv.reset();
}
// save latest reset counters
_vxy_reset_counter = vehicle_local_position.vxy_reset_counter;
_vz_reset_counter = vehicle_local_position.vz_reset_counter;
_xy_reset_counter = vehicle_local_position.xy_reset_counter;
_z_reset_counter = vehicle_local_position.z_reset_counter;
_heading_reset_counter = vehicle_local_position.heading_reset_counter;
// 检查当前位置是否有效，将local_pos(当前位置、速度、加速度数据)以向量形式保存到states结构体
PositionControlStates states{set_vehicle_states(vehicle_local_position)};
if (_vehicle_control_mode.flag_multicopter_position_control_enabled) {
// set failsafe setpoint if there hasn't been a new
// trajectory setpoint since position control started
if ((_setpoint.timestamp < _time_position_control_enabled)
&& (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) {
_setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, false);
}
}
if (_vehicle_control_mode.flag_multicopter_position_control_enabled
&& (_setpoint.timestamp >= _time_position_control_enabled)) {
// update vehicle constraints and handle smooth takeoff
_vehicle_constraints_sub.update(&_vehicle_constraints);
// fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN
// TODO: this should get obsolete once the takeoff limiting moves into the flight tasks
if (!PX4_ISFINITE(_vehicle_constraints.speed_up) || (_vehicle_constraints.speed_up > _param_mpc_z_vel_max_up.get())) {
_vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get();
}
if (_vehicle_control_mode.flag_control_offboard_enabled) {
const bool want_takeoff = _vehicle_control_mode.flag_armed
&& (vehicle_local_position.timestamp_sample < _setpoint.timestamp + 1_s);
if (want_takeoff && PX4_ISFINITE(_setpoint.position[2])
&& (_setpoint.position[2] < states.position(2))) {
_vehicle_constraints.want_takeoff = true;
} else if (want_takeoff && PX4_ISFINITE(_setpoint.velocity[2])
&& (_setpoint.velocity[2] < 0.f)) {
_vehicle_constraints.want_takeoff = true;
} else if (want_takeoff && PX4_ISFINITE(_setpoint.acceleration[2])
&& (_setpoint.acceleration[2] < 0.f)) {
_vehicle_constraints.want_takeoff = true;
} else {
_vehicle_constraints.want_takeoff = false;
}
// override with defaults
_vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get();
_vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
}
// handle smooth takeoff// handle smooth takeoff 平稳起飞
_takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed,
_vehicle_constraints.want_takeoff,
_vehicle_constraints.speed_up, false, vehicle_local_position.timestamp_sample);
const bool not_taken_off = (_takeoff.getTakeoffState() < TakeoffState::rampup);
const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight);
const bool flying_but_ground_contact = (flying && _vehicle_land_detected.ground_contact);
if (!flying) {
_control.setHoverThrust(_param_mpc_thr_hover.get());
}
// make sure takeoff ramp is not amended by acceleration feed-forward
if (_takeoff.getTakeoffState() == TakeoffState::rampup && PX4_ISFINITE(_setpoint.velocity[2])) {
_setpoint.acceleration[2] = NAN;
}
if (not_taken_off || flying_but_ground_contact) {
// we are not flying yet and need to avoid any corrections
_setpoint = PositionControl::empty_trajectory_setpoint;
_setpoint.timestamp = vehicle_local_position.timestamp_sample;
Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
// prevent any integrator windup
_control.resetIntegral();
}
// limit tilt during takeoff ramupup
// 分别设置飞行器在起飞期间和升到一定高度后倾斜角度限制
// 在起飞期间：防止其在空中倾斜过度而失去平衡
// 当飞行器升起到一定高度后，就可以放宽倾斜角度的限制，以便更好地完成任务。
const float tilt_limit_deg = (_takeoff.getTakeoffState() < TakeoffState::flight)
? _param_mpc_tiltmax_lnd.get() : _param_mpc_tiltmax_air.get();
// 对倾斜角度进行平滑处理，以避免飞行器突然改变倾斜角度而导致不稳定
// 然后将限制后的倾斜角度值设置到控制器中
_control.setTiltLimit(_tilt_limit_slew_rate.update(math::radians(tilt_limit_deg), dt));
const float speed_up = _takeoff.updateRamp(dt,
PX4_ISFINITE(_vehicle_constraints.speed_up) ? _vehicle_constraints.speed_up : _param_mpc_z_vel_max_up.get());
const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down :
_param_mpc_z_vel_max_dn.get();
// Allow ramping from zero thrust on takeoff
// 设置最小推力，还外起飞：0，已经起飞：系统设置的最小油门值
const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f;
// 推力限幅
_control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get());
float max_speed_xy = _param_mpc_xy_vel_max.get();
if (PX4_ISFINITE(vehicle_local_position.vxy_max)) {
max_speed_xy = math::min(max_speed_xy, vehicle_local_position.vxy_max);
}
// 水平速度、上升速度、下降速度限幅
_control.setVelocityLimits(
max_speed_xy,
math::min(speed_up, _param_mpc_z_vel_max_up.get()), // takeoff ramp starts with negative velocity limit
math::max(speed_down, 0.f));
// 设置控制输入（期望的位置、速度、加速度和姿态等信息），用于后续控制器中的计算和决策，进行调整并控制飞行器的运动
_control.setInputSetpoint(_setpoint);
// update states
if (!PX4_ISFINITE(_setpoint.position[2])
&& PX4_ISFINITE(_setpoint.velocity[2]) && (fabsf(_setpoint.velocity[2]) > FLT_EPSILON)
&& PX4_ISFINITE(vehicle_local_position.z_deriv) && vehicle_local_position.z_valid && vehicle_local_position.v_z_valid) {
// A change in velocity is demanded and the altitude is not controlled.
// Set velocity to the derivative of position
// because it has less bias but blend it in across the landing speed range
// < MPC_LAND_SPEED: ramp up using altitude derivative without a step
// >= MPC_LAND_SPEED: use altitude derivative
float weighting = fminf(fabsf(_setpoint.velocity[2]) / _param_mpc_land_speed.get(), 1.f);
states.velocity(2) = vehicle_local_position.z_deriv * weighting + vehicle_local_position.vz * (1.f - weighting);
}
// 设置控制器当前的状态,其中包括位置、速度、偏航角和速度变化率信息
_control.setState(states);
// Run position control
//主程序
if (!_control.update(dt)) {
// Failsafe
_vehicle_constraints = {0, NAN, NAN, false, {}}; // reset constraints
//如果位置控制器成功更新控制输入并生成新的姿态控制指令，则重置失控保护状态否则会进入失控保护模式
_control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, true));
_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
_control.update(dt);
}
// Publish internal position control setpoints
// on top of the input/feed-forward setpoints these containt the PID corrections
// This message is used by other modules (such as Landdetector) to determine vehicle intention.
vehicle_local_position_setpoint_s local_pos_sp{};
_control.getLocalPositionSetpoint(local_pos_sp);
local_pos_sp.timestamp = hrt_absolute_time();
_local_pos_sp_pub.publish(local_pos_sp);
// Publish attitude setpoint output
vehicle_attitude_setpoint_s attitude_setpoint{};
_control.getAttitudeSetpoint(attitude_setpoint);
attitude_setpoint.timestamp = hrt_absolute_time();
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
} else {
// an update is necessary here because otherwise the takeoff state doesn't get skipped with non-altitude-controlled modes
_takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, true,
vehicle_local_position.timestamp_sample);
}
// Publish takeoff status
const uint8_t takeoff_state = static_cast<uint8_t>(_takeoff.getTakeoffState());
if (takeoff_state != _takeoff_status_pub.get().takeoff_state
|| !isEqualF(_tilt_limit_slew_rate.getState(), _takeoff_status_pub.get().tilt_limit)) {
_takeoff_status_pub.get().takeoff_state = takeoff_state;
_takeoff_status_pub.get().tilt_limit = _tilt_limit_slew_rate.getState();
_takeoff_status_pub.get().timestamp = hrt_absolute_time();
_takeoff_status_pub.update();
}
}
perf_end(_cycle_perf);
}
bool PositionControl::update(const float dt)
{
// 通过检查输入设置点是否有效，判断控制器的输入是否合法
bool valid = _inputValid();
if (valid) {
// 位置控制计算：使无人机沿着设定位置以期望速度飞行，并且通过速度限制保证无人机飞行安全。
_positionControl();
// 速度控制计算
_velocityControl(dt);
// 对输入的期望偏航速度和期望偏航角进行赋值，如果为 NaN，那么设置为 0
_yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f;
// 如果期望偏航角为 NaN，那么将其设置为当前偏航角。
_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control
}
// There has to be a valid output acceleration and thrust setpoint otherwise something went wrong
return valid && _acc_sp.isAllFinite() && _thr_sp.isAllFinite();
}
void PositionControl::_positionControl()
{
// P-position controller
// 根据位置误差和位置环P参数计算期望速度（NED系）
// 1、位置偏差=（期望位置 - 当前位置）
// 2、emult() 函数表示向量元素之间的按元素乘积
// 3、期望速度 = 位置偏差 * 比例增益
Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p);
// Position and feed-forward velocity setpoints or position states being NAN results in them not having an influence
// 1、将期望速度输出（vel_sp_position）与 速度前馈输入（_vel_sp ）相加，得到总期望
// 如果 _vel_sp 和 vel_sp_position 都不为NAN，则相加： _vel_sp += vel_sp_position；
// 如果 _vel_sp 是NAN，则不相加： _vel_sp = vel_sp_position；
ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position);
// make sure there are no NAN elements for further reference while constraining
// 确保位置控制输出（vel_sp_position）中不含NAN，避免出现计算错误
// 检查每个分量是否为NaN，如果是则将其替换为零
ControlMath::setZeroIfNanVector3f(vel_sp_position);
// Constrain horizontal velocity by prioritizing the velocity component along the
// the desired position setpoint over the feed-forward term.
// 将总体期望速度的水平分量限制在指定的范围内
// 官方注释：通过在前馈项上优先考虑沿所需位置设定点的速度分量来约束水平速度。没太理解！！！
_vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal);
// Constrain velocity in z-direction.
// 限制垂直方向的速度幅值在设定的上下限内
_vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down);
}
void PositionControl::_velocityControl(const float dt)
{
// Constrain vertical velocity integral
_vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G);
// PID velocity control
// 计算速度误差
Vector3f vel_error = _vel_sp - _vel;
// 使用PID控制器计算期望加速度
// 速度误差与比例常数向量相乘，产生一个比例调节向量，控制比例控制器的增益，以确保输出的响应对误差的大小有合适的补偿
// _vel_int：速度误差的积分项：由每个时间步的误差累积得到
// _vel_dot：速度导数（替代加速度估计）：由每个时间步的速度累积得到
// _vel_dot.emult(_gain_vel_d)微分项：_vel_dot与微分常数向量 _gain_vel_d 元素相乘，产生一个微分调节向量
// 微分控制器的作用是通过对速度变化率的响应来减少超调和震荡，以使系统的动态响应更加平滑和稳定
// 三个部分相加，就得到了加速度的期望值
Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);
// No control input from setpoints or corresponding states which are NAN
// 速度误差产生的期望加速度和期望加速度前馈叠加，作为总的期望加速度，原理同位置控制
ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);
// 加速度控制：根据速度设定值和加速度设定值计算期望推力
_accelerationControl();
// Integrator anti-windup in vertical direction
// 垂直方向积分器抗饱和:在飞行器的垂直控制中，油门值往往对应着飞行器的垂直速度，因此限制油门值可以起到控制飞行器垂直速度的作用。
// 通过限制油门值的范围,同时满足速度误差的方向和油门值的方向一致，
// 来限制积分项的增长，从而避免积分饱和
// -_lim_thr_min:由于采用的是NED坐标系，即指向地面为正，而升力是向上的，所以是负的
if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.f) ||
(_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.f)) {
vel_error(2) = 0.f;
}
// Prioritize vertical control while keeping a horizontal margin
// 限制最大垂直推力以保持稳定
const Vector2f thrust_sp_xy(_thr_sp);
const float thrust_sp_xy_norm = thrust_sp_xy.norm();
// 限制水平最大推力
/*
这个开平方的方法是用来求出无人机在给定最大总推力限制下，能够提供的最大水平推力量的大小。
具体而言，我们可以把无人机总推力限制看作一个圆柱体的高度，注意这里：总推力的限制并不一定
沿着垂直方向，而是指在所有方向上的推力限制之和。
而水平推力限制看作圆柱体的底面积，
那么无人机总推力限制和水平推力限制就可以确定这个圆柱体的大小。在这种情况下，
我们需要计算的是圆柱体底面的最大面积，也就是能够提供的最大水平推力量。通过勾股定理可以知道，
在给定总推力限制和当前沿垂直方向的推力量的情况下，水平推力量的最大值就是圆柱体底面的直径的一半，
也就是圆柱体底面积的平方根。因此，我们需要对计算出来的底面积和总推力量分别进行平方，
然后取它们的差值的平方根，就能得到最大水平推力量的大小。
*/
const float thrust_max_squared = math::sq(_lim_thr_max);
// Determine how much vertical thrust is left keeping horizontal margin
const float allocated_horizontal_thrust = math::min(thrust_sp_xy_norm, _lim_thr_xy_margin);
const float thrust_z_max_squared = thrust_max_squared - math::sq(allocated_horizontal_thrust);
// Saturate maximal vertical thrust
_thr_sp(2) = math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared));
// Determine how much horizontal thrust is left after prioritizing vertical control
const float thrust_max_xy_squared = thrust_max_squared - math::sq(_thr_sp(2));
float thrust_max_xy = 0;
if (thrust_max_xy_squared > 0) {
thrust_max_xy = sqrtf(thrust_max_xy_squared);
}
// Saturate thrust in horizontal direction
if (thrust_sp_xy_norm > thrust_max_xy) {
_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
}
// 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_produced = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);
const float arw_gain = 2.f / _gain_vel_p(0);
// The produced acceleration can be greater or smaller than the desired acceleration due to the saturations and the actual vertical thrust (computed independently).
// The ARW loop needs to run if the signal is saturated only.
const Vector2f acc_sp_xy = _acc_sp.xy();
const Vector2f acc_limited_xy = (acc_sp_xy.norm_squared() > acc_sp_xy_produced.norm_squared())
? acc_sp_xy_produced
: acc_sp_xy;
vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_limited_xy);
// 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;
}
void PositionControl::_accelerationControl()
{
// Assume standard acceleration due to gravity in vertical direction for attitude generation
// 计算期望的机身Z轴方向（应该朝向重力加速度的反方向）
// 这里假设垂直方向只有重力加速度，通过normalized（）函数将合力向量单位化，以确保其模长为1
Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();
// 将姿态限制在最大倾角内，以确保无人机的稳定性
// 如何限制：机体Z轴向量和单位化的地理Z轴向量进行点乘，求得两个向量的余弦，再求反余弦得到倾斜角度，然后对角度进行限幅
ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);
// 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
// Vector3f(0, 0, 1)是一个指向世界坐标系中竖直向上的单位向量。
// body_z是一个指向机体坐标系中竖直向上的单位向量。
// 两个向量点积运算得到它们在空间中的夹角的余弦值。因为两个向量都是单位向量，它们的点积就是它们夹角的余弦值
// 总推力 / 竖直向上和机体z轴向之间的夹角的余弦值 = 机体z轴方向上的总推力（可以画图理解，比较简单）
collective_thrust /= (Vector3f(0, 0, 1).dot(body_z));
// 总推力限幅
collective_thrust = math::min(collective_thrust, -_lim_thr_min);
// 将机体z轴方向上的总推力投影回世界坐标系中，得到期望的推力向量_thr_sp
// 注意body_z是单位向量
_thr_sp = body_z * collective_thrust;
}
1259
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
