// land_nogps_run - runs the land controller
// pilot controls roll and pitch angles
// should be called at 100hz or more
void Copter::land_nogps_run()
{
float target_roll = 0.0f, target_pitch = 0.0f;
float target_yaw_rate = 0;
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum
if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
init_disarm_motors();
}
#else
// disarm when the landing detector says we've landed
if (ap.land_complete) {
init_disarm_motors();
}
#endif
return;
}
// process pilot inputs
if (!failsafe.radio) {
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// get pilot desired lean angles
get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch);
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// pause 4 seconds before beginning land descent
float cmb_rate;
if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) {
cmb_rate = 0;
} else {
land_pause = false;
cmb_rate = get_land_descent_speed();
}
// record desired climb rate for logging
desired_climb_rate = cmb_rate;
// call position controller
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
}
// failsafe_gcs_check - check for ground station failsafe
void Sub::failsafe_gcs_check()
{
// return immediately if we have never had contact with a gcs, or if gcs failsafe action is disabled
// this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state.
if (failsafe.last_heartbeat_ms == 0 || (!g.failsafe_gcs && g.failsafe_gcs == FS_GCS_DISABLED)) {
return;
}
uint32_t tnow = AP_HAL::millis();
// Check if we have gotten a GCS heartbeat recently (GCS sysid must match SYSID_MYGCS parameter)
if (tnow < failsafe.last_heartbeat_ms + FS_GCS_TIMEOUT_MS) {
// Log event if we are recovering from previous gcs failsafe
if (failsafe.gcs) {
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
}
failsafe.gcs = false;
return;
}
//////////////////////////////
// GCS heartbeat has timed out
//////////////////////////////
// Send a warning every 30 seconds
if (tnow > failsafe.last_gcs_warn_ms + 30000) {
failsafe.last_gcs_warn_ms = tnow;
gcs().send_text(MAV_SEVERITY_WARNING, "MYGCS: %d, heartbeat lost", g.sysid_my_gcs);
}
// do nothing if we have already triggered the failsafe action, or if the motors are disarmed
if (failsafe.gcs || !motors.armed()) {
return;
}
// update state, log to dataflash
failsafe.gcs = true;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_OCCURRED);
// handle failsafe action
if (g.failsafe_gcs == FS_GCS_DISARM) {
init_disarm_motors();
} else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) {
if (!set_mode(ALT_HOLD, MODE_REASON_GCS_FAILSAFE)) {
init_disarm_motors();
}
} else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) {
if (!set_mode(SURFACE, MODE_REASON_GCS_FAILSAFE)) {
init_disarm_motors();
}
}
}
// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds
// called at 1hz
void Copter::auto_disarm_check()
{
uint8_t disarm_delay;
// exit immediately if we are already disarmed or throttle output is not zero,
if (!motors.armed() || !ap.throttle_zero) {
auto_disarming_counter = 0;
return;
}
// allow auto disarm in manual flight modes or Loiter/AltHold if we're landed
// always allow auto disarm if using interlock switch or motors are Emergency Stopped
if (mode_has_manual_throttle(control_mode) || ap.land_complete || (ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) {
auto_disarming_counter++;
// use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less
// obvious the copter is armed as the motors will not be spinning
if (ap.using_interlock || ap.motor_emergency_stop){
disarm_delay = AUTO_DISARMING_DELAY_SHORT;
} else {
disarm_delay = AUTO_DISARMING_DELAY_LONG;
}
if(auto_disarming_counter >= disarm_delay) {
init_disarm_motors();
auto_disarming_counter = 0;
}
}else{
auto_disarming_counter = 0;
}
}
/*
* This event will be called when the failsafe changes
* boolean failsafe reflects the current state
*/
void Copter::failsafe_radio_on_event()
{
// if motors are not armed there is nothing to do
if( !motors.armed() ) {
return;
}
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else {
if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
// continue mission
} else if (control_mode == LAND && g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
// continue landing
} else {
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
} else {
set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
}
}
}
// log the error to the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_OCCURRED);
}
void Copter::failsafe_battery_event(void)
{
// return immediately if low battery event has already been triggered
if (failsafe.battery) {
return;
}
// failsafe check
if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) {
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else {
if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) {
set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
} else {
set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
}
}
}
// set the low battery flag
set_failsafe_battery(true);
// warn the ground station and log to dataflash
gcs_send_text(MAV_SEVERITY_WARNING,"Low battery");
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
}
void Sub::failsafe_sensors_check(void)
{
if (!ap.depth_sensor_present) {
return;
}
// We need a depth sensor to do any sort of auto z control
if (sensor_health.depth) {
failsafe.sensor_health = false;
return;
}
// only report once
if (failsafe.sensor_health) {
return;
}
failsafe.sensor_health = true;
gcs().send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!");
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_SENSORS, ERROR_CODE_BAD_DEPTH);
if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) {
// This should always succeed
if (!set_mode(MANUAL, MODE_REASON_BAD_DEPTH)) {
// We should never get here
init_disarm_motors();
}
}
}
// rtl_returnhome_run - return home
// called by rtl_run at 100hz or more
void Copter::rtl_land_run()
{
// if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// set target to current position
wp_nav.init_loiter_target();
// disarm when the landing detector says we've landed
if (ap.land_complete) {
init_disarm_motors();
}
// check if we've completed this stage of RTL
rtl_state_complete = ap.land_complete;
return;
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
land_run_horizontal_control();
land_run_vertical_control();
// check if we've completed this stage of RTL
rtl_state_complete = ap.land_complete;
}
// Make sure that we are receiving pilot input at an appropriate interval
void Sub::failsafe_pilot_input_check()
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
if (g.failsafe_pilot_input == FS_PILOT_INPUT_DISABLED) {
failsafe.pilot_input = false;
return;
}
if (AP_HAL::millis() < failsafe.last_pilot_input_ms + g.failsafe_pilot_input_timeout * 1000.0f) {
failsafe.pilot_input = false; // We've received an update from the pilot within the timeout period
return;
}
if (failsafe.pilot_input) {
return; // only act once
}
failsafe.pilot_input = true;
Log_Write_Error(ERROR_SUBSYSTEM_INPUT, ERROR_CODE_FAILSAFE_OCCURRED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Lost manual control");
set_neutral_controls();
if(g.failsafe_pilot_input == FS_PILOT_INPUT_DISARM) {
init_disarm_motors();
}
#endif
}
// land_run - runs the land controller
// horizontal position controlled with loiter controller
// should be called at 100hz or more
void Copter::land_gps_run()
{
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
wp_nav.init_loiter_target();
// disarm when the landing detector says we've landed
if (ap.land_complete) {
init_disarm_motors();
}
return;
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// pause before beginning land descent
if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
land_pause = false;
}
land_run_horizontal_control();
land_run_vertical_control(land_pause);
}
// exit_mission - function that is called once the mission completes
void Sub::exit_mission()
{
// play a tone
AP_Notify::events.mission_complete = 1;
init_disarm_motors();
}
// land_nogps_run - runs the land controller
// pilot controls roll and pitch angles
// should be called at 100hz or more
void Copter::land_nogps_run()
{
float target_roll = 0.0f, target_pitch = 0.0f;
float target_yaw_rate = 0;
// process pilot inputs
if (!failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high
set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
}
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// get pilot desired lean angles
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// disarm when the landing detector says we've landed
if (ap.land_complete) {
init_disarm_motors();
}
return;
}
// 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(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// pause before beginning land descent
if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
land_pause = false;
}
land_run_vertical_control(land_pause);
}
// arm_motors_check - checks for pilot input to arm or disarm the copter
// called at 10hz
void Copter::arm_motors_check()
{
static int16_t arming_counter;
// ensure throttle is down
if (channel_throttle->control_in > 0) {
arming_counter = 0;
return;
}
int16_t tmp = channel_yaw->control_in;
// full right
if (tmp > 4000) {
// increase the arming counter to a maximum of 1 beyond the auto trim counter
if( arming_counter <= AUTO_TRIM_DELAY ) {
arming_counter++;
}
// arm the motors and configure for flight
if (arming_counter == ARM_DELAY && !motors.armed()) {
// reset arming counter if arming fail
if (!init_arm_motors(false)) {
arming_counter = 0;
}
}
// arm the motors and configure for flight
if (arming_counter == AUTO_TRIM_DELAY && motors.armed() && control_mode == STABILIZE) {
auto_trim_counter = 250;
// ensure auto-disarm doesn't trigger immediately
auto_disarm_begin = millis();
}
// full left
}else if (tmp < -4000) {
if (!mode_has_manual_throttle(control_mode) && !ap.land_complete) {
arming_counter = 0;
return;
}
// increase the counter to a maximum of 1 beyond the disarm delay
if( arming_counter <= DISARM_DELAY ) {
arming_counter++;
}
// disarm the motors
if (arming_counter == DISARM_DELAY && motors.armed()) {
init_disarm_motors();
}
// Yaw is centered so reset arming counter
}else{
arming_counter = 0;
}
}
// Check for a crash
// The vehicle is considered crashed if the angle error exceeds a specified limit for more than 2 seconds
void Sub::failsafe_crash_check()
{
static uint32_t last_crash_check_pass_ms;
uint32_t tnow = AP_HAL::millis();
// return immediately if disarmed, or crash checking disabled
if (!motors.armed() || g.fs_crash_check == FS_CRASH_DISABLED) {
last_crash_check_pass_ms = tnow;
failsafe.crash = false;
return;
}
// return immediately if we are not in an angle stabilized flight mode
if (control_mode == ACRO || control_mode == MANUAL) {
last_crash_check_pass_ms = tnow;
failsafe.crash = false;
return;
}
// check for angle error over 30 degrees
const float angle_error = attitude_control.get_att_error_angle_deg();
if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
last_crash_check_pass_ms = tnow;
failsafe.crash = false;
return;
}
if (tnow < last_crash_check_pass_ms + CRASH_CHECK_TRIGGER_MS) {
return;
}
// Conditions met, we are in failsafe
// Send warning to GCS
if (tnow > failsafe.last_crash_warn_ms + 20000) {
failsafe.last_crash_warn_ms = tnow;
gcs().send_text(MAV_SEVERITY_WARNING,"Crash detected");
}
// Only perform failsafe action once
if (failsafe.crash) {
return;
}
failsafe.crash = true;
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
// disarm motors
if (g.fs_crash_check == FS_CRASH_DISARM) {
init_disarm_motors();
}
}
// parachute_release - trigger the release of the parachute, disarm the motors and notify the user
void Copter::parachute_release()
{
// send message to gcs and dataflash
gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Released");
Log_Write_Event(DATA_PARACHUTE_RELEASED);
// disarm motors
init_disarm_motors();
// release parachute
parachute.release();
}
void Sub::failsafe_ekf_check(void)
{
static uint32_t last_ekf_good_ms = 0;
if (g.fs_ekf_action == FS_EKF_ACTION_DISABLED) {
last_ekf_good_ms = AP_HAL::millis();
failsafe.ekf = false;
AP_Notify::flags.ekf_bad = false;
return;
}
float posVar, hgtVar, tasVar;
Vector3f magVar;
Vector2f offset;
float compass_variance;
float vel_variance;
ahrs.get_variances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
compass_variance = magVar.length();
if (compass_variance < g.fs_ekf_thresh && vel_variance < g.fs_ekf_thresh) {
last_ekf_good_ms = AP_HAL::millis();
failsafe.ekf = false;
AP_Notify::flags.ekf_bad = false;
return;;
}
// Bad EKF for 2 solid seconds triggers failsafe
if (AP_HAL::millis() < last_ekf_good_ms + 2000) {
failsafe.ekf = false;
AP_Notify::flags.ekf_bad = false;
return;
}
// Only trigger failsafe once
if (failsafe.ekf) {
return;
}
failsafe.ekf = true;
AP_Notify::flags.ekf_bad = true;
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE);
if (AP_HAL::millis() > failsafe.last_ekf_warn_ms + 20000) {
failsafe.last_ekf_warn_ms = AP_HAL::millis();
gcs().send_text(MAV_SEVERITY_WARNING, "EKF bad");
}
if (g.fs_ekf_action == FS_EKF_ACTION_DISARM) {
init_disarm_motors();
}
}
// failsafe_gcs_check - check for ground station failsafe
void Copter::failsafe_gcs_check()
{
uint32_t last_gcs_update_ms;
// return immediately if gcs failsafe is disabled, gcs has never been connected or we are not overriding rc controls from the gcs and we are not in guided mode
// this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state.
if ((!failsafe.gcs)&&(g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0 || (!failsafe.rc_override_active && control_mode != GUIDED))) {
return;
}
// calc time since last gcs update
// note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs
last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms;
// check if all is well
if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS) {
// check for recovery from gcs failsafe
if (failsafe.gcs) {
failsafe_gcs_off_event();
set_failsafe_gcs(false);
}
return;
}
// do nothing if gcs failsafe already triggered or motors disarmed
if (failsafe.gcs || !motors->armed()) {
return;
}
// GCS failsafe event has occurred
// update state, log to dataflash
set_failsafe_gcs(true);
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_OCCURRED);
// clear overrides so that RC control can be regained with radio.
hal.rcin->clear_overrides();
failsafe.rc_override_active = false;
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else {
if (control_mode == AUTO && g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
// continue mission
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
set_mode_SmartRTL_or_RTL(MODE_REASON_GCS_FAILSAFE);
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
set_mode_SmartRTL_or_land_with_pause(MODE_REASON_GCS_FAILSAFE);
} else { // g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL
set_mode_RTL_or_land_with_pause(MODE_REASON_GCS_FAILSAFE);
}
}
}
// exit_mission - function that is called once the mission completes
void Copter::exit_mission()
{
// play a tone
AP_Notify::events.mission_complete = 1;
// if we are not on the ground switch to loiter or land
if(!ap.land_complete) {
// try to enter loiter but if that fails land
if(!auto_loiter_start()) {
set_mode(LAND, MODE_REASON_MISSION_END);
}
}else{
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum
if (ap.throttle_zero || failsafe.radio) {
init_disarm_motors();
}
#else
// if we've landed it's safe to disarm
init_disarm_motors();
#endif
}
}
// terrain failsafe action
void Copter::failsafe_terrain_on_event()
{
failsafe.terrain = true;
gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing");
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_FAILSAFE_OCCURRED);
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else if (control_mode == RTL) {
mode_rtl.restart_without_terrain();
} else {
set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE);
}
}
// brake_run - runs the brake controller
// should be called at 100hz or more
void Copter::brake_run()
{
// if not auto armed set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-throttle_average);
return;
}
// relax stop target if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// if landed immediately disarm
if (ap.land_complete) {
init_disarm_motors();
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run brake controller
wp_nav.update_brake(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);
// body-frame rate controller is run directly from 100hz loop
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
pos_control.update_z_controller();
if (brake_timeout_ms != 0 && millis()-brake_timeout_start >= brake_timeout_ms) {
if (!set_mode(LOITER, MODE_REASON_BRAKE_TIMEOUT)) {
set_mode(ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT);
}
}
}
// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds
void Copter::auto_disarm_check()
{
uint32_t tnow_ms = millis();
uint32_t disarm_delay_ms = 1000*constrain_int16(g.disarm_delay, 0, 127);
// exit immediately if we are already disarmed, or if auto
// disarming is disabled
if (!motors.armed() || disarm_delay_ms == 0) {
auto_disarm_begin = tnow_ms;
return;
}
#if FRAME_CONFIG == HELI_FRAME
// if the rotor is still spinning, don't initiate auto disarm
if (motors.rotor_speed_above_critical()) {
auto_disarm_begin = tnow_ms;
return;
}
#endif
// always allow auto disarm if using interlock switch or motors are Emergency Stopped
if ((ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) {
#if FRAME_CONFIG != HELI_FRAME
// use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less
// obvious the copter is armed as the motors will not be spinning
disarm_delay_ms /= 2;
#endif
} else {
bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0;
bool thr_low;
if (mode_has_manual_throttle(control_mode) || !sprung_throttle_stick) {
thr_low = ap.throttle_zero;
} else {
float deadband_top = g.rc_3.get_control_mid() + g.throttle_deadzone;
thr_low = g.rc_3.control_in <= deadband_top;
}
if (!thr_low || !ap.land_complete) {
// reset timer
auto_disarm_begin = tnow_ms;
}
}
// disarm once timer expires
if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) {
init_disarm_motors();
auto_disarm_begin = tnow_ms;
}
}
// fence_check - ask fence library to check for breaches and initiate the response
// called at 1hz
void Copter::fence_check()
{
const uint8_t orig_breaches = fence.get_breaches();
// check for new breaches; new_breaches is bitmask of fence types breached
const uint8_t new_breaches = fence.check();
// we still don't do anything when disarmed, but we do check for fence breaches.
// fence pre-arm check actually checks if any fence has been breached
// that's not ever going to be true if we don't call check on AP_Fence while disarmed.
if (!motors->armed()) {
return;
}
// if there is a new breach take action
if (new_breaches) {
// if the user wants some kind of response and motors are armed
if(fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY ) {
// disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
// don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
if (ap.land_complete || (flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){
init_disarm_motors();
} else {
// if always land option mode is specified, land
if (fence.get_action() == AC_FENCE_ACTION_ALWAYS_LAND) {
set_mode(LAND, MODE_REASON_FENCE_BREACH);
} else if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
set_mode(LAND, MODE_REASON_FENCE_BREACH);
// if we are within 100m of the fence, RTL
}
} else {
// if more than 100m outside the fence just force a land
set_mode(LAND, MODE_REASON_FENCE_BREACH);
}
}
}
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, new_breaches);
} else if (orig_breaches) {
// record clearing of breach
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, ERROR_CODE_ERROR_RESOLVED);
}
}
// parachute_release - trigger the release of the parachute, disarm the motors and notify the user
void Sub::parachute_release()
{
// send message to gcs and dataflash
gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Released");
Log_Write_Event(DATA_PARACHUTE_RELEASED);
// disarm motors
init_disarm_motors();
// release parachute
parachute.release();
// deploy landing gear
landinggear.set_cmd_mode(LandingGear_Deploy);
}
// exit_mission - function that is called once the mission completes
void Copter::exit_mission()
{
// play a tone
AP_Notify::events.mission_complete = 1;
// if we are not on the ground switch to loiter or land
if(!ap.land_complete) {
// try to enter loiter but if that fails land
if(!auto_loiter_start()) {
set_mode(LAND, MODE_REASON_MISSION_END);
}
}else{
// if we've landed it's safe to disarm
init_disarm_motors();
}
}
// fence_check - ask fence library to check for breaches and initiate the response
// called at 1hz
void Copter::fence_check()
{
uint8_t new_breaches; // the type of fence that has been breached
uint8_t orig_breaches = fence.get_breaches();
// give fence library our current distance from home in meters
fence.set_home_distance(home_distance*0.01f);
// check for a breach
new_breaches = fence.check_fence(current_loc.alt/100.0f);
// return immediately if motors are not armed
if(!motors.armed()) {
return;
}
// if there is a new breach take action
if( new_breaches != AC_FENCE_TYPE_NONE ) {
// if the user wants some kind of response and motors are armed
if(fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY ) {
// disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
// don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
if (ap.land_complete || (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){
init_disarm_motors();
}else{
// if we are within 100m of the fence, RTL
if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
set_mode(LAND, MODE_REASON_FENCE_BREACH);
}
}else{
// if more than 100m outside the fence just force a land
set_mode(LAND, MODE_REASON_FENCE_BREACH);
}
}
}
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, new_breaches);
}
// record clearing of breach
if(orig_breaches != AC_FENCE_TYPE_NONE && fence.get_breaches() == AC_FENCE_TYPE_NONE) {
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, ERROR_CODE_ERROR_RESOLVED);
}
}
// Battery failsafe handler
void Sub::handle_battery_failsafe(const char* type_str, const int8_t action)
{
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
switch((Failsafe_Action)action) {
case Failsafe_Action_Surface:
set_mode(SURFACE, MODE_REASON_BATTERY_FAILSAFE);
break;
case Failsafe_Action_Disarm:
init_disarm_motors();
break;
case Failsafe_Action_Warn:
case Failsafe_Action_None:
break;
}
}
// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds
// called at 1hz
void Copter::auto_disarm_check()
{
uint8_t disarm_delay = constrain_int16(g.disarm_delay, 0, 127);
// exit immediately if we are already disarmed, or if auto
// disarming is disabled
if (!motors.armed() || disarm_delay == 0) {
auto_disarming_counter = 0;
return;
}
// always allow auto disarm if using interlock switch or motors are Emergency Stopped
if ((ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) {
auto_disarming_counter++;
#if FRAME_CONFIG != HELI_FRAME
// use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less
// obvious the copter is armed as the motors will not be spinning
disarm_delay /= 2;
#endif
} else {
bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0;
bool thr_low;
if (mode_has_manual_throttle(control_mode) || !sprung_throttle_stick) {
thr_low = ap.throttle_zero;
} else {
float deadband_top = g.rc_3.get_control_mid() + g.throttle_deadzone;
thr_low = g.rc_3.control_in <= deadband_top;
}
if (thr_low && ap.land_complete) {
// increment counter
auto_disarming_counter++;
} else {
// reset counter
auto_disarming_counter = 0;
}
}
// disarm once counter expires
if (auto_disarming_counter >= disarm_delay) {
init_disarm_motors();
auto_disarming_counter = 0;
}
}
// Battery failsafe check
// Check the battery voltage and remaining capacity
void Sub::failsafe_battery_check(void)
{
// Do nothing if the failsafe is disabled, or if we are disarmed
if (g.failsafe_battery_enabled == FS_BATT_DISABLED || !motors.armed()) {
failsafe.battery = false;
AP_Notify::flags.failsafe_battery = false;
return; // Failsafe disabled, nothing to do
}
if (!battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
failsafe.battery = false;
AP_Notify::flags.failsafe_battery = false;
return; // Battery is doing well
}
// Always warn when failsafe condition is met
if (AP_HAL::millis() > failsafe.last_battery_warn_ms + 20000) {
failsafe.last_battery_warn_ms = AP_HAL::millis();
gcs_send_text(MAV_SEVERITY_WARNING, "Low battery");
}
// Don't do anything if failsafe has already been set
if (failsafe.battery) {
return;
}
failsafe.battery = true;
AP_Notify::flags.failsafe_battery = true;
// Log failsafe
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
switch(g.failsafe_battery_enabled) {
case FS_BATT_SURFACE:
set_mode(SURFACE, MODE_REASON_BATTERY_FAILSAFE);
break;
case FS_BATT_DISARM:
init_disarm_motors();
break;
default:
break;
}
}
// crash_check - disarms motors if a crash has been detected
// crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second
// called at MAIN_LOOP_RATE
void Copter::crash_check()
{
static uint16_t crash_counter; // number of iterations vehicle may have been crashed
// return immediately if disarmed, or crash checking disabled
if (!motors.armed() || ap.land_complete || g.fs_crash_check == 0) {
crash_counter = 0;
return;
}
// return immediately if we are not in an angle stabilize flight mode or we are flipping
if (control_mode == ACRO || control_mode == FLIP) {
crash_counter = 0;
return;
}
// vehicle not crashed if 1hz filtered acceleration is more than 3m/s (1G on Z-axis has been subtracted)
if (land_accel_ef_filter.get().length() >= CRASH_CHECK_ACCEL_MAX) {
crash_counter = 0;
return;
}
// check for angle error over 30 degrees
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
if (pythagorous2(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) {
crash_counter = 0;
return;
}
// we may be crashing
crash_counter++;
// check if crashing for 2 seconds
if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * MAIN_LOOP_RATE)) {
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
// send message to gcs
gcs_send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming");
// disarm motors
init_disarm_motors();
hal.uartA->printf("CRASH!");
}
}
// Recovery failed, take action
void Sub::failsafe_terrain_act()
{
switch (g.failsafe_terrain) {
case FS_TERRAIN_HOLD:
if (!set_mode(POSHOLD, MODE_REASON_TERRAIN_FAILSAFE)) {
set_mode(ALT_HOLD, MODE_REASON_TERRAIN_FAILSAFE);
}
AP_Notify::events.failsafe_mode_change = 1;
break;
case FS_TERRAIN_SURFACE:
set_mode(SURFACE, MODE_REASON_TERRAIN_FAILSAFE);
AP_Notify::events.failsafe_mode_change = 1;
break;
case FS_TERRAIN_DISARM:
default:
init_disarm_motors();
}
}
// brake_run - runs the brake controller
// should be called at 100hz or more
void Copter::brake_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-throttle_average);
return;
}
// relax stop target if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// if landed immediately disarm
if (ap.land_complete) {
init_disarm_motors();
}
// run brake controller
wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f);
// body-frame rate controller is run directly from 100hz loop
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate(0.0f, G_Dt, false);
pos_control.update_z_controller();
}
