开头说两句
手动控制的核心在此。
再说两个名词
local frame:相当于b系吧(我感觉是,不确定)
再整一个框架
- 前期准备开始
- 前期准备结束
- 由推杆获取b系下的速度设定值
- 根据公式:v_n = C_n_b * v_b获取n系下的速度设定值
- 第一个if:水平轴
if(flag_control_position_enabled) { }
else { };- 第二个if:垂直轴
if(flag_control_altitude_enabled) { }
else { };- 第三个if:着陆后
if(_vehicle_land_detected){ }
else { };
再看 control_manual(dt) 源码
void
MulticopterPositionControl::control_manual(float dt)
{
/* ========================================= 前期准备开始 ======================================== */
/* Entering manual control from non-manual control mode,
* reset alt/pos setpoints
* 由非手动控制切换到手动控制,重置高度和位置设定值 */
if (_mode_auto) {
_mode_auto = false;
/* Reset alt pos flags if resetting is enabled
* 如果可以的话,复位高度和位置标志位*/
if (_do_reset_alt_pos_flag) {
_reset_pos_sp = true;
_reset_alt_sp = true;
}
}
/* ======================================== 前期准备结束 ========================================= */
/* 由推杆获取b系下的速度设定值 */
/* X,Y in local frame and Z in global (D), in [-1,1] normalized range
* X,Y在机体坐标系,Z在大地坐标系,均被量化在[-1,1] */
/* req_vel_sp 是期望速度设定值 */
math::Vector<3> req_vel_sp;
/* 将其初始化为0向量 */
req_vel_sp.zero();
/* 以下两个if为 req_vel_sp 赋值 */
if (_control_mode.flag_control_altitude_enabled) {
/* set vertical velocity setpoint with throttle stick
* 通过油门杆设定垂直速度设定值*/
req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy);
}
if (_control_mode.flag_control_position_enabled) {
/* set horizontal velocity setpoint with roll/pitch stick
* 通过横滚航线推杆设定XY向速度设定值*/
req_vel_sp(0) = _manual.x;
req_vel_sp(1) = _manual.y;
}
/* req_vel_sp 赋值完成 */
/* 推杆给 req_vel_sp 赋值完成后,重置高度和位置设定值为当前状态下的值 */
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
* 限制速度设定值在[-1.0 , 1.0] */
float req_vel_sp_norm = req_vel_sp.length();
if (req_vel_sp_norm > 1.0f) {
req_vel_sp /= req_vel_sp_norm;
}
/* 至此,b系下的速度设定值已经计算完毕,为 req_vel_sp,它是一个3×1的矩阵 */
/* 根据公式:v_n = C_n_b * v_b获取n系下的速度设定值 */
/* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw
*将_req_vel_sp量化为0-1,将其缩放到最大速度并绕航向旋转,这是什么鬼 */
/* R_yaw_sp为旋转矩阵,即C_n_b */
math::Matrix<3, 3> R_yaw_sp;
/* 通过欧拉角得到旋转矩阵 */
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
/* n系下的速度 = C_n_b * b系下的速度
* req_vel_sp_scaled 为在n系下的速度,即我们观测到的速度
* 以下为计算n系下的速度 */
math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(
_params.vel_cruise); // in NED and scaled to actual velocity
/* 至此,n系下的速度设定值已经计算完毕,它是一个3×1的矩阵 */
/* assisted velocity mode: user controls velocity, but if velocity is small enough, position
* hold is activated for the corresponding axis
* 辅助速度模式:用户控制速度,但是如果速度足够小,则相应轴的位置保持被激活 */
/* ========================================== 第一个if ========================================== */
/* horizontal axes
* 水平轴 */
if (_control_mode.flag_control_position_enabled) {
/* check for pos. hold
* 检查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) {
float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1));
if (_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy) {
/* reset position setpoint to have smooth transition
* from velocity control to position control
* 重置位置设定值,以便于能够平稳地由速度控制模式转换到位置控制模式 */
_pos_hold_engaged = true;
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
} 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);
/* request velocity setpoint to be used, instead of position setpoint
* “要求的速度设定值”将被使用,而非位置设定值 */
_run_pos_control = false;
_vel_sp(0) = req_vel_sp_scaled(0);
_vel_sp(1) = req_vel_sp_scaled(1);
}
}
/* ========================================== 第二个if ========================================== */
/* vertical axis
* 垂直轴 */
if (_control_mode.flag_control_altitude_enabled) {
/* check for pos. hold
* 检查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) {
/* reset position setpoint to have smooth transition
*from velocity control to position control
* 重置位置设定值,以便于能够平稳地由速度控制模式转换为位置控制模式 */
_alt_hold_engaged = true;
_pos_sp(2) = _pos(2);
} else {
_alt_hold_engaged = false;
}
}
} else {
_alt_hold_engaged = false;
_pos_sp(2) = _pos(2);
}
/* set requested velocity setpoint
* 设置需要的速度设定值 */
if (!_alt_hold_engaged) {
/* request velocity setpoint to be used, instead of altitude setpoint
* “要求的速度设定值”将被使用,而非高度设定值 */
_run_alt_control = false;
_vel_sp(2) = req_vel_sp_scaled(2);
}
}
/* ========================================== 第三个if ========================================== */
if (_vehicle_land_detected.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_setpoint.identity();
_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();
} else {
control_position(dt);
}
}
写在最后
本程序输出的是:
_vel_sp(0)
_vel_sp(1)
_vel_sp(2)
分别对应XYZ方向的速度设定值。