throw模式
此模式存在一定的危险性，该模式允许飞行器抛向空中来启动电机，当在空中时，该模式不再接受任何输入信息，需要使用GPS
使用方法
1、飞机正常上电，并等待GPS搜星。此时飞机未解锁
2、切换到throw模式
3、检查GPS是否已经定位
4、解锁飞行器，飞行器蜂鸣器处于待命状态，电机默认情况下不旋转
5、将飞行器抛出（爬升速度达到0.5m/s，速度达到5m/s）
6、飞行器稳定后，切换到其他模式重新获得控制权
参数设置
THROW_TYPE：0表示往上扔飞行器，1表示往下扔，如果是往下扔，高度至少10m；
THROW_MOT_START：控制电机在待机时是不转还是缓慢旋转（0不转、1缓慢旋转），默认为0；
THROW_NEXTMODE：设置飞行器停止后的下一个飞行模式（Auto、Guided、RTL、Land和Brake模式均支持），设置为Throw（默认）便保持在该模式等待手动切换到其他模式。
源码分析
先看初始化：
// runs the throw to start controller
// should be called at 100hz or more
void Copter::throw_run()
{
/* Throw State Machine
Throw_Disarmed - motors are off
Throw_Detecting - motors are on and we are waiting for the throw
Throw_Uprighting - the throw has been detected and the copter is being uprighted
Throw_HgtStabilise - the copter is kept level and height is stabilised about the target height
Throw_PosHold - the copter is kept at a constant position and height
/
/抛射起飞的几个标志
* 未解锁时：
* 电机解锁，等待被扔
* 被扔出去了
* 飞机保持高度
* 飞机能够定点
*/
// Don't enter THROW mode if interlock will prevent motors running
if (!motors->armed() && motors->get_interlock()) { //电机已经解锁&&安全开关保护中
// state machine entry is always from a disarmed state
throw_state.stage = Throw_Disarmed;
} else if (throw_state.stage == Throw_Disarmed && motors->armed()) { //电机解锁后等待抛飞
gcs().send_text(MAV_SEVERITY_INFO,"waiting for throw"); //向地面站通知等待throw
throw_state.stage = Throw_Detecting; //进入throw探测阶段，等待被扔
} else if (throw_state.stage == Throw_Detecting && throw_detected()){//探测到自己已经被扔出去了
gcs().send_text(MAV_SEVERITY_INFO,"throw detected - uprighting"); //向地面站发送消息
throw_state.stage = Throw_Uprighting; //进入throw调整阶段
// Cancel the waiting for throw tone sequence
AP_Notify::flags.waiting_for_throw = false;
} else if (throw_state.stage == Throw_Uprighting && throw_attitude_good()) {
gcs().send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
throw_state.stage = Throw_HgtStabilise; //进入高度保持阶段
// initialize vertical speed and acceleration limits
// use brake mode values for rapid response
pos_control->set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z); //初始化速度和加速度的限制，使用刹车时的那个值作为可响应范围
pos_control->set_accel_z(BRAKE_MODE_DECEL_RATE);
// initialise the demanded height to 3m above the throw height
// we want to rapidly clear surrounding obstacles
if (g2.throw_type == ThrowType_Drop) { //如果是向下抛，目标高度比解锁时的高度再低一米
pos_control->set_alt_target(inertial_nav.get_altitude() - 100);
} else { //如果是向上抛，目标高度是解锁时的高度再高3米
pos_control->set_alt_target(inertial_nav.get_altitude() + 300);
}
// set the initial velocity of the height controller demand to the measured velocity if it is going up
// if it is going down, set it to zero to enforce a very hard stop
pos_control->set_desired_velocity_z(fmaxf(inertial_nav.get_velocity_z(),0.0f));
//设置z方向的目标速度，如果是向上抛需要使用到测量的速度，如果向下抛的话就直接设置成0（这里还不是很明白）
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
set_auto_armed(true); //自动解锁
} else if (throw_state.stage == Throw_HgtStabilise && throw_height_good()) {
gcs().send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
throw_state.stage = Throw_PosHold; //高度控制好之后开始控制位置
// initialise the loiter target to the curent position and velocity
wp_nav->init_loiter_target(); //调用loiter模式
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
set_auto_armed(true); //设置自动解锁，可是上面已经设置过一次了，可能是为了意想不到的情况吧
} else if (throw_state.stage == Throw_PosHold && throw_position_good()) {
if (!throw_state.nextmode_attempted) {
switch (g2.throw_nextmode) { //定点完成之后进入下一个模式，可以在参数列表中设置THROW_NEXTMODE
case AUTO:
case GUIDED:
case RTL:
case LAND:
case BRAKE:
set_mode((control_mode_t)g2.throw_nextmode.get(), MODE_REASON_THROW_COMPLETE);
break;
default: //如果使用默认的就是定点在当前位置，等待手动切换模式
// do nothing
break;
}
throw_state.nextmode_attempted = true;
}
}
// Throw State Processing
switch (throw_state.stage) {
case Throw_Disarmed: //throw模式飞机解锁
// prevent motors from rotating before the throw is detected unless enabled by the user
if (g.throw_motor_start == 1) { //控制待机时电机不转，此参数默认是0
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
}
// demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);//设置油门输出为0，禁用增稳？（不太明白，求解答）
break;
case Throw_Detecting:
// prevent motors from rotating before the throw is detected unless enabled by the user
if (g.throw_motor_start == 1) { //控制待机时电机不转，和上面一样
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
}
// Hold throttle at zero during the throw and continually reset the attitude controller
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
// Play the waiting for throw tone sequence to alert the user
AP_Notify::flags.waiting_for_throw = true;
break;
case Throw_Uprighting:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// demand a level roll/pitch attitude with zero yaw rate
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain()); //努力保持水平
// output 50% throttle and turn off angle boost to maximise righting moment
attitude_control->set_throttle_out(0.5f, false, g.throttle_filt);
break;
case Throw_HgtStabilise:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); //电机输出范围为全范围
// call attitude controller //调用姿态控制器
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
// call height controller //调用高度控制器
pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
pos_control->update_z_controller();
break;
case Throw_PosHold:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); //同样是全油门范围开启只为能够定了点的你
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); //调用定点控制器
// call attitude controller //调用姿态控制器
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f, get_smoothing_gain());
// call height controller
pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); //设置目标爬升率为0
pos_control->update_z_controller(); //更新z轴控制器
break;
}
// log at 10hz or if stage changes
uint32_t now = AP_HAL::millis(); //飞行日志的记录
if ((throw_state.stage != throw_state.prev_stage) || (now - throw_state.last_log_ms) > 100) {
throw_state.prev_stage = throw_state.stage;
throw_state.last_log_ms = now;
float velocity = inertial_nav.get_velocity().length();
float velocity_z = inertial_nav.get_velocity().z;
float accel = ins.get_accel().length();
float ef_accel_z = ahrs.get_accel_ef().z;
bool throw_detect = (throw_state.stage > Throw_Detecting) || throw_detected();
bool attitude_ok = (throw_state.stage > Throw_Uprighting) || throw_attitude_good();
bool height_ok = (throw_state.stage > Throw_HgtStabilise) || throw_height_good();
bool pos_ok = (throw_state.stage > Throw_PosHold) || throw_position_good();
Log_Write_Throw(throw_state.stage,
velocity,
velocity_z,
accel,
ef_accel_z,
throw_detect,
attitude_ok,
height_ok,
pos_ok);
}
}
模块化的好处就是实现一个新的模式，大多数只需要把其他的东西拼凑结合一下。
抛飞主要分了几个阶段，在解锁时的自检是通用的，在检测抛飞时不断地检测传感器的数据，在知道已经被扔出去之后利用姿态控制和位置控制的程序实现悬停，然后再用一个set_mode()就切换到了下一个模式。
网友评论