// initiate user takeoff - called when MAVLink TAKEOFF command is received
bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
{
if (motors->armed() && ap.land_complete && current_mode_has_user_takeoff(must_navigate) && takeoff_alt_cm > current_loc.alt) {
#if FRAME_CONFIG == HELI_FRAME
// Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning
if (!motors->rotor_runup_complete()) {
return false;
}
#endif
switch(control_mode) {
case GUIDED:
if (mode_guided.takeoff_start(takeoff_alt_cm)) {
set_auto_armed(true);
return true;
}
return false;
case LOITER:
case POSHOLD:
case ALT_HOLD:
case SPORT:
set_auto_armed(true);
takeoff_timer_start(takeoff_alt_cm);
return true;
default:
return false;
}
}
return false;
}
// update_auto_armed - update status of auto_armed flag
void Copter::update_auto_armed()
{
// disarm checks
if(ap.auto_armed){
// if motors are disarmed, auto_armed should also be false
if(!motors.armed()) {
set_auto_armed(false);
return;
}
// if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
if(mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio) {
set_auto_armed(false);
}
}else{
// arm checks
#if FRAME_CONFIG == HELI_FRAME
// for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
if(motors.armed() && !ap.throttle_zero && motors.rotor_runup_complete()) {
set_auto_armed(true);
}
#else
// if motors are armed and throttle is above zero auto_armed should be true
if(motors.armed() && !ap.throttle_zero) {
set_auto_armed(true);
}
#endif // HELI_FRAME
}
}
// set guided mode angle target
void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads)
{
// check we are in velocity control mode
if (guided_mode != Guided_Angle) {
guided_angle_control_start();
}
// convert quaternion to euler angles
q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);
guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;
guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;
guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
guided_angle_state.yaw_rate_cds = ToDeg(yaw_rate_rads) * 100.0f;
guided_angle_state.use_yaw_rate = use_yaw_rate;
guided_angle_state.climb_rate_cms = climb_rate_cms;
guided_angle_state.update_time_ms = millis();
// interpret positive climb rate as triggering take-off
if (motors.armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
set_auto_armed(true);
}
// log target
Log_Write_GuidedTarget(guided_mode,
Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms));
}
// initiate user takeoff - called when MAVLink TAKEOFF command is received
bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
{
if (motors.armed() && ap.land_complete && current_mode_has_user_takeoff(must_navigate) && takeoff_alt_cm > current_loc.alt) {
switch(control_mode) {
case GUIDED:
set_auto_armed(true);
guided_takeoff_start(takeoff_alt_cm);
return true;
case LOITER:
case POSHOLD:
case ALT_HOLD:
case SPORT:
set_auto_armed(true);
takeoff_timer_start(takeoff_alt_cm);
return true;
}
}
return false;
}
// update_auto_armed - update status of auto_armed flag
void Sub::update_auto_armed()
{
// disarm checks
if (ap.auto_armed) {
// if motors are disarmed, auto_armed should also be false
if (!motors.armed()) {
set_auto_armed(false);
return;
}
// if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
if (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.manual_control) {
set_auto_armed(false);
}
} else {
// arm checks
// if motors are armed and throttle is above zero auto_armed should be true
if (motors.armed()) {
set_auto_armed(true);
}
}
}
// update_auto_armed - update status of auto_armed flag
void Copter::update_auto_armed()
{
// disarm checks
if(ap.auto_armed){
// if motors are disarmed, auto_armed should also be false
if(!motors.armed()) {
set_auto_armed(false);
return;
}
// if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
if(mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio) {
set_auto_armed(false);
}
#if FRAME_CONFIG == HELI_FRAME
// if helicopters are on the ground, and the motor is switched off, auto-armed should be false
// so that rotor runup is checked again before attempting to take-off
if(ap.land_complete && !motors.rotor_runup_complete()) {
set_auto_armed(false);
}
#endif // HELI_FRAME
}else{
// arm checks
#if FRAME_CONFIG == HELI_FRAME
// for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
if(motors.armed() && !ap.throttle_zero && motors.rotor_runup_complete()) {
set_auto_armed(true);
}
#else
// if motors are armed and throttle is above zero auto_armed should be true
// if motors are armed and we are in throw mode, then auto_ermed should be true
if(motors.armed() && (!ap.throttle_zero || control_mode == THROW)) {
set_auto_armed(true);
}
#endif // HELI_FRAME
}
}
// runs the throw to start controller
// should be called at 100hz or more
void Copter::throw_run()
{
static ThrowModeState throw_state = Throw_Disarmed;
/* 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 = Throw_Disarmed;
// remember the current value of the motor interlock so that this condition can be restored if we exit the throw mode before starting motors
throw_early_exit_interlock = true;
// prevent motors from rotating before the throw is detected unless enabled by the user
if (g.throw_motor_start == 1) {
motors.set_interlock(true);
} else {
motors.set_interlock(false);
}
// status to let system know flight control has not started which means the interlock setting needs to restored if we exit to another flight mode
// this is necessary because throw mode uses the interlock to achieve a post arm motor start.
throw_flight_commenced = false;
} else if (throw_state == Throw_Disarmed && motors.armed()) {
gcs_send_text(MAV_SEVERITY_INFO,"waiting for throw");
throw_state = Throw_Detecting;
// prevent motors from rotating before the throw is detected unless enabled by the user
if (g.throw_motor_start == 1) {
motors.set_interlock(true);
} else {
motors.set_interlock(false);
}
} else if (throw_state == Throw_Detecting && throw_detected()){
gcs_send_text(MAV_SEVERITY_INFO,"throw detected - uprighting");
throw_state = Throw_Uprighting;
// Cancel the waiting for throw tone sequence
AP_Notify::flags.waiting_for_throw = false;
// reset the interlock
motors.set_interlock(true);
// status to let system know flight control has started which means the entry interlock setting will not restored if we exit to another flight mode
throw_flight_commenced = true;
} else if (throw_state == Throw_Uprighting && throw_attitude_good()) {
gcs_send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
throw_state = 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
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));
// 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 == Throw_HgtStabilise && throw_height_good()) {
gcs_send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
throw_state = Throw_PosHold;
// initialise the loiter target to the curent position and velocity
wp_nav.init_loiter_target();
// 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);
}
// Throw State Processing
switch (throw_state) {
case Throw_Disarmed:
// 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);
motors.slow_start(true);
break;
case Throw_Detecting:
// 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:
// 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);
// output 50% throttle and turn off angle boost to maximise righting moment
attitude_control.set_throttle_out(500, false, g.throttle_filt);
break;
case Throw_HgtStabilise:
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
// 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:
// 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);
// call height controller
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
pos_control.update_z_controller();
break;
}
}
// 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_state.stage = Throw_Detecting;
} else if (throw_state.stage == Throw_Detecting && throw_detected()){
gcs_send_text(MAV_SEVERITY_INFO,"throw detected - uprighting");
throw_state.stage = Throw_Uprighting;
// 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 {
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));
// 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();
// 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) {
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:
// 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);
}
// 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);
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(500, 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);
pos_control.update_z_controller();
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);
}
}
