- control_position(dt)
void
MulticopterPositionControl::control_position(float dt)
{
/* 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);
}
/* make sure velocity setpoint is saturated in xy
* 确保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_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
* 确保z向速度设定值饱和 */
if (_vel_sp(2) < -1.0f * _params.vel_max_up) {
_vel_sp(2) = -1.0f * _params.vel_max_up;
}
if (_vel_sp(2) > _params.vel_max_down) {
_vel_sp(2) = _params.vel_max_down;
}
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;
}
if (!_control_mode.flag_control_climb_rate_enabled) {
_vel_sp(2) = 0.0f;
}
/* TODO: remove this is a pathetic leftover, it's here just to make sure that
* _takeoff_jumped flags are reset
* TODO：删除。这是一个无价值的剩余物，它出现在这里只是确保_takeoff_jumped标志位被重置*/
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) {
_takeoff_jumped = false;
_takeoff_thrust_sp = 0.0f;
}
limit_acceleration(dt);
_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);
}
if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled) {
/* reset integrals if needed
* 如果需要，重置积分项 */
if (_control_mode.flag_control_climb_rate_enabled) {
if (_reset_int_z) {
_reset_int_z = false;
_thrust_int(2) = 0.0f;
}
} 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;
/* thrust vector in NED frame
* 在北东地大地坐标系下的推力向量 */
math::Vector<3> thrust_sp;
if (_control_mode.flag_control_acceleration_enabled && _pos_sp_triplet.current.acceleration_valid) {
thrust_sp = math::Vector<3>(_pos_sp_triplet.current.a_x, _pos_sp_triplet.current.a_y, _pos_sp_triplet.current.a_z);
} else {
thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d)
+ _thrust_int - math::Vector<3>(0.0f, 0.0f, _params.thr_hover);
}
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 && !_control_mode.flag_control_acceleration_enabled) {
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
}
/* if still or already on ground command zero xy velcoity and
* zero xy thrust_sp in body frame to consider uneven ground
* 如果依然或者早已经在地面上，则把机体坐标系下的xy向的速度和推力设定值归零，以防止停在不平坦的地面上 */
if (_vehicle_land_detected.ground_contact) {
/* thrust setpoint in body frame
* 机体坐标系下的推力设定值 */
math::Vector<3> thrust_sp_body = _R.transposed() * thrust_sp;
/* we dont want to make any correction in body x and y
* 我们不想对机身的xy进行任何修正 */
thrust_sp_body(0) = 0.0f;
thrust_sp_body(1) = 0.0f;
/* make sure z component of thrust_sp_body is larger than 0
* (positive thrust is downward)
* 确保thrust_sp_body在Z向的分量大于0，主动的推力是向下的 */
thrust_sp_body(2) = thrust_sp(2) > 0.0f ? thrust_sp(2) : 0.0f;
/* convert back to local frame (NED)
* 转换回大地坐标系 */
thrust_sp = _R * thrust_sp_body;
/* set velocity setpoint to zero and reset position
* 设置速度设定值为0并重置位置 */
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
}
if (!_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_acceleration_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 tilt_max = _params.tilt_max_air;
float thr_max = _params.thr_max;
/* filter vel_z over 1/8sec
* 每1/8s 对z向速度进行滤波 */
_vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2);
/* filter vel_z change over 1/8sec
* 每1/8秒 对z向速度变化进行滤波 */
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;
/* We can only run the control if we're already in-air, have a takeoff setpoint,
* or if we're in offboard control.
* Otherwise, we should just bail out
* 如果我们早已在空中，我们只能运行控制器，除非我们在offboard控制模式下，否则我们需要一个起飞设定值，
* 否则，。。。*/
const bool got_takeoff_setpoint = (_pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) ||
_control_mode.flag_control_offboard_enabled;
if (_vehicle_land_detected.landed && !got_takeoff_setpoint) {
/* Keep throttle low while still on ground.
*如果依然在地上，保持低油门 */
thr_max = 0.0f;
} else if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
/* adjust limits for landing mode
* 调整着陆模式的限制 */
/* 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)fabsf(_acc_z_lp) < 0.1f
&& _vel_z_lp > 0.6f * _params.land_speed) {
_in_landing = true;
}
float land_z_threshold = 0.1f;
/* assume ground, cut thrust
* 到达地面，切断推力 */
if (_in_landing
&& _vel_z_lp < land_z_threshold) {
thr_max = 0.0f;
_in_landing = false;
_lnd_reached_ground = true;
} else if (_in_landing
&& _vel_z_lp < math::min(0.3f * _params.land_speed, 2.5f * land_z_threshold)) {
/* not on ground but with ground contact, stop position and velocity control
* 不在地面，但是又地面接触，停止位置和速度控制 */
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
_vel_sp(0) = _vel(0);
_vel_sp(1) = _vel(1);
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
}
/* 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
* XXX：奇异值，我们假定自由落体加速度超过4m/s2 */
&& (_acc_z_lp > 4.0f
|| _vel_z_lp > 2.0f * _params.land_speed)) {
thr_max = _params.thr_max;
_in_landing = true;
_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;
/* Don't freeze altitude integral if it wants to throttle up
* 如果要调大油门，就不要固定高度的积分项 */
saturation_z = vel_err(2) > 0.0f ? true : saturation_z;
}
if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {
/* limit max tilt
* 限制最大倾斜角 */
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;
/* Don't freeze x,y integrals if they both want to throttle down
* 如果要调小油门，就不要固定xy的积分项 */
saturation_xy = ((vel_err(0) * _vel_sp(0) < 0.0f) && (vel_err(1) * _vel_sp(1) < 0.0f)) ? saturation_xy : true;
}
}
}
}
if (_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_velocity_enabled) {
/* thrust compensation when vertical velocity but not horizontal velocity is controlled
* 竖直方向有推力补偿，而水平方向没有 */
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;
}
/* Calculate desired total thrust amount in body z direction.
* 计算机体坐标系z向的期望总推力 */
/* To compensate for excess thrust during attitude tracking errors we
* project the desired thrust force vector F onto the real vehicle's thrust axis in NED:
* body thrust axis [0,0,-1]' rotated by R is: R*[0,0,-1]' = -R_z
* 为了在姿态跟踪期间补偿过大的推力，我们将地理坐标系NED中的期望推力向量F投影到机体坐标系
* 机体推力轴[0,0,-1]'由R旋转得到：R*[0,0,-1]' = -R_z */
matrix::Vector3f R_z(_R(0, 2), _R(1, 2), _R(2, 2));
matrix::Vector3f F(thrust_sp.data);
/* recalculate because it might have changed
* 之所以重新计算，是因为它可能会被改写 */
float thrust_body_z = F.dot(-R_z);
/* limit max thrust
* 限制最大推力 */
if (fabsf(thrust_body_z) > thr_max) {
if (thrust_sp(2) < 0.0f) {
if (-thrust_sp(2) > thr_max) {
/* thrust Z component is too large, limit it
* 推力Z的比重太大，限制之 */
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
thrust_sp(2) = -thr_max;
saturation_xy = true;
/* Don't freeze altitude integral if it wants to throttle down
* 当要调小油门时，不要限制高度的积分项 */
saturation_z = vel_err(2) < 0.0f ? true : saturation_z;
} else {
/* preserve thrust Z component and lower XY,
*keeping altitude is more important than position
* 保持Z向推力的比重并降低xy向推力的比重，因为保持高度比保持位置更重要 */
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;
/* Don't freeze x,y integrals if they both want to throttle down
* 当要降低xy向的油门时，不要限制积分项 */
saturation_xy = ((vel_err(0) * _vel_sp(0) < 0.0f) && (vel_err(1) * _vel_sp(1) < 0.0f)) ? saturation_xy : true;
}
} else {
/* Z component is positive, going down (Z is positive down in NED),
* simply limit thrust vector
* z向分量为正则下降（在北东地坐标系中z的正向向下），知识限制推力向量 */
float k = thr_max / fabsf(thrust_body_z);
thrust_sp *= k;
saturation_xy = true;
saturation_z = true;
}
thrust_body_z = thr_max;
}
_att_sp.thrust = math::max(thrust_body_z, thr_min);
/* 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;
}
/* calculate attitude setpoint from thrust vector
* 由推力向量计算高度设定值 */
if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {
/* desired body_z axis = -normalize(thrust_vector)
* 期望的body_z axis = -normalize(推力向量) */
math::Vector<3> body_x;
math::Vector<3> body_y;
math::Vector<3> body_z;
if (thrust_sp.length() > SIGMA) {
body_z = -thrust_sp.normalized();
} else {
/* no thrust, set Z axis to safe value
* 没有推力则设定Z轴为安全值 */
body_z.zero();
body_z(2) = 1.0f;
}
/* vector of desired yaw direction in XY plane, rotated by PI/2
* 期望航向的方向矢量在xy平面内，旋转PI/2 */
math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
if (fabsf(body_z(2)) > SIGMA) {
/* desired body_x axis, orthogonal to body_z
* 期望的body_x轴正交于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
* 期望的推力在xy平面内，设定下面的X来构造正确的矩阵，但是航向部分实际上是不会被用的 */
body_x.zero();
body_x(2) = 1.0f;
}
/* desired body_y axis
* 期望的body_y轴 */
body_y = body_z % body_x;
/* fill rotation matrix
* 填写旋转矩阵 */
for (int i = 0; i < 3; i++) {
_R_setpoint(i, 0) = body_x(i);
_R_setpoint(i, 1) = body_y(i);
_R_setpoint(i, 2) = body_z(i);
}
/* copy quaternion setpoint to attitude setpoint topic
* 将四元数设定值复制到设定值框架中 */
matrix::Quatf q_sp = _R_setpoint;
memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d));
_att_sp.q_d_valid = true;
/* calculate euler angles, for logging only, must not be used for control
* 计算欧拉角，仅用于记录，不能用于控制 */
matrix::Eulerf euler = _R_setpoint;
_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
* 航向已经用于构造旋转矩阵，但实际上旋转矩阵在奇异点附近有不同的航向 */
} else if (!_control_mode.flag_control_manual_enabled) {
/* autonomous altitude control without position control (failsafe landing),
* force level attitude, don't change yaw
* 无位置控制的自动高度控制（安全着陆失败）在不改变航向的前提下，强制控制水平姿态 */
_R_setpoint = matrix::Eulerf(0.0f, 0.0f, _att_sp.yaw_body);
/* copy quaternion setpoint to attitude setpoint topic
* 将四元数复制到姿态控制的框架中 */
matrix::Quatf q_sp = _R_setpoint;
memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d));
_att_sp.q_d_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
}
/* save thrust setpoint for logging
* 为记录日志保存推力设定值 */
_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;
}
}
5万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
