// exit_mode - high level call to organise cleanup as a flight mode is exited
void Sub::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
{
#if AUTOTUNE_ENABLED == ENABLED
if (old_control_mode == AUTOTUNE) {
autotune_stop();
}
#endif
// stop mission when we leave auto mode
if (old_control_mode == AUTO) {
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
}
#if MOUNT == ENABLED
camera_mount.set_mode_to_default();
#endif // MOUNT == ENABLED
}
if (old_control_mode == THROW) {
throw_exit();
}
// smooth throttle transition when switching from manual to automatic flight modes
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->control_in));
}
// cancel any takeoffs in progress
takeoff_stop();
}
// exit_mode - high level call to organise cleanup as a flight mode is exited
void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode)
{
#if AUTOTUNE_ENABLED == ENABLED
if (old_control_mode == AUTOTUNE) {
autotune_stop();
}
#endif
// stop mission when we leave auto mode
if (old_control_mode == AUTO) {
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
}
#if MOUNT == ENABLED
camera_mount.set_mode_to_default();
#endif // MOUNT == ENABLED
}
// smooth throttle transition when switching from manual to automatic flight modes
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors->armed() && !ap.land_complete) {
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
set_accel_throttle_I_from_pilot_throttle();
}
// cancel any takeoffs in progress
takeoff_stop();
// call smart_rtl cleanup
if (old_control_mode == SMART_RTL) {
smart_rtl_exit();
}
#if FRAME_CONFIG == HELI_FRAME
// firmly reset the flybar passthrough to false when exiting acro mode.
if (old_control_mode == ACRO) {
attitude_control->use_flybar_passthrough(false, false);
motors->set_acro_tail(false);
}
// if we are changing from a mode that did not use manual throttle,
// stab col ramp value should be pre-loaded to the correct value to avoid a twitch
// heli_stab_col_ramp should really only be active switching between Stabilize and Acro modes
if (!mode_has_manual_throttle(old_control_mode)){
if (new_control_mode == STABILIZE){
input_manager.set_stab_col_ramp(1.0);
} else if (new_control_mode == ACRO){
input_manager.set_stab_col_ramp(0.0);
}
}
#endif //HELI_FRAME
}
// 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
}
}
// 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;
}
}
// mode_allows_arming - returns true if vehicle can be armed in the specified mode
// arming_from_gcs should be set to true if the arming request comes from the ground station
bool Sub::mode_allows_arming(control_mode_t mode, bool arming_from_gcs)
{
if (mode_has_manual_throttle(mode) || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) {
return true;
}
return false;
}
// update estimated throttle required to hover (if necessary)
// called at 100hz
void Copter::update_throttle_hover()
{
#if FRAME_CONFIG != HELI_FRAME
// if not armed or landed exit
if (!motors.armed() || ap.land_complete) {
return;
}
// do not update in manual throttle modes or Drift
if (mode_has_manual_throttle(control_mode) || (control_mode == DRIFT)) {
return;
}
// do not update while climbing or descending
if (!is_zero(pos_control.get_desired_velocity().z)) {
return;
}
// get throttle output
float throttle = motors.get_throttle();
// calc average throttle if we are in a level hover
if (throttle > 0.0f && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
// Can we set the time constant automatically
motors.update_throttle_hover(0.01f);
}
#endif
}
// circle_init - initialise circle controller flight mode
bool Copter::circle_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
// as this will force the helicopter to descend.
if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
return false;
}
#endif
if (position_ok() || ignore_checks) {
circle_pilot_yaw_override = false;
// initialize speeds and accelerations
pos_control.set_speed_xy(wp_nav.get_speed_xy());
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise circle controller including setting the circle center based on vehicle speed
circle_nav.init();
return true;
}else{
return false;
}
}
// mode_allows_arming - returns true if vehicle can be armed in the specified mode
// arming_from_gcs should be set to true if the arming request comes from the ground station
bool Copter::mode_allows_arming(control_mode_t mode, bool arming_from_gcs)
{
if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == SPORT || mode == THROW || (arming_from_gcs && (mode == GUIDED || mode == GUIDED_NOGPS))) {
return true;
}
return false;
}
// auto_init - initialise auto controller
bool Copter::auto_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
// as this will force the helicopter to descend.
if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
return false;
}
#endif
if ((position_ok() && mission.num_commands() > 1) || ignore_checks) {
auto_mode = Auto_Loiter;
// stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
if (auto_yaw_mode == AUTO_YAW_ROI) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init();
// clear guided limits
guided_limit_clear();
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
return true;
}else{
return false;
}
}
// 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;
}
}
// acro_init - initialise acro controller
bool Sub::acro_init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (g.rc_3.control_in > get_non_takeoff_throttle())) {
return false;
}
// set target altitude to zero for reporting
pos_control.set_alt_target(0);
return true;
}
// acro_init - initialise acro controller
bool Copter::acro_init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors->armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&
(get_pilot_desired_throttle(channel_throttle->get_control_in(), g2.acro_thr_mid) > get_non_takeoff_throttle())) {
return false;
}
// set target altitude to zero for reporting
pos_control->set_alt_target(0);
return true;
}
// exit_mode - high level call to organise cleanup as a flight mode is exited
void Copter::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
{
#if AUTOTUNE_ENABLED == ENABLED
if (old_control_mode == AUTOTUNE) {
autotune_stop();
}
#endif
// stop mission when we leave auto mode
if (old_control_mode == AUTO) {
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
}
#if MOUNT == ENABLED
camera_mount.set_mode_to_default();
#endif // MOUNT == ENABLED
}
// smooth throttle transition when switching from manual to automatic flight modes
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->control_in));
}
// cancel any takeoffs in progress
takeoff_stop();
#if FRAME_CONFIG == HELI_FRAME
// firmly reset the flybar passthrough to false when exiting acro mode.
if (old_control_mode == ACRO) {
attitude_control.use_flybar_passthrough(false, false);
motors.set_acro_tail(false);
}
// reset RC Passthrough to motors
motors.reset_radio_passthrough();
#endif //HELI_FRAME
}
// 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()
{
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);
}
}
// 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;
}
}
// 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);
}
}
}
void Sub::set_land_complete(bool b)
{
// if no change, exit immediately
if( ap.land_complete == b )
return;
land_detector_count = 0;
if(b){
Log_Write_Event(DATA_LAND_COMPLETE);
} else {
Log_Write_Event(DATA_NOT_LANDED);
}
ap.land_complete = b;
// trigger disarm-on-land if configured
bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0;
bool mode_disarms_on_land = mode_allows_arming(control_mode,false) && !mode_has_manual_throttle(control_mode);
if (ap.land_complete && motors.armed() && disarm_on_land_configured && mode_disarms_on_land) {
init_disarm_motors();
}
}
// 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
}
}
// arm_checks - perform final checks before arming
// always called just before arming. Return true if ok to arm
// has side-effect that logging is started
bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
{
#if LOGGING_ENABLED == ENABLED
// start dataflash
start_logging();
#endif
// check accels and gyro are healthy
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
if(!ins.get_accel_health_all()) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Accelerometers not healthy"));
}
return false;
}
if(!ins.get_gyro_health_all()) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Gyros not healthy"));
}
return false;
}
// get ekf attitude (if bad, it's usually the gyro biases)
if (!pre_arm_ekf_attitude_check()) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: gyros still settling"));
}
return false;
}
}
// always check if inertial nav has started and is ready
if(!ahrs.healthy()) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Waiting for Nav Checks Arm"));
}
return false;
}
if(compass.is_calibrating()) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Compass calibration running"));
}
return false;
}
// always check if the current mode allows arming
if (!mode_allows_arming(control_mode, arming_from_gcs)) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Mode not armable"));
}
return false;
}
// always check gps
if (!pre_arm_gps_checks(display_failure)) {
return false;
}
// heli specific arming check
#if FRAME_CONFIG == HELI_FRAME
// check if rotor is spinning on heli because this could disrupt gyro calibration
if (!motors.allow_arming()){
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Rotor is Spinning")); }
return false;
}
#endif // HELI_FRAME
// succeed if arming checks are disabled
if (g.arming_check == ARMING_CHECK_NONE) {
return true;
}
// baro checks
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
// baro health check
if (!barometer.all_healthy()) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Barometer not healthy"));
}
return false;
}
// Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
// Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
// that may differ from the baro height due to baro drift.
nav_filter_status filt_status = inertial_nav.get_filter_status();
bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
if (using_baro_ref && (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Altitude disparity"));
}
return false;
}
}
#if AC_FENCE == ENABLED
// check vehicle is within fence
if(!fence.pre_arm_check()) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: check fence"));
}
return false;
}
#endif
// check lean angle
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > aparm.angle_max) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Leaning"));
}
return false;
}
}
// check battery voltage
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Check Battery"));
}
return false;
}
}
// check throttle
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) {
// check throttle is not too low - must be above failsafe throttle
if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Collective below Failsafe"));
#else
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Throttle below Failsafe"));
#endif
}
return false;
}
// check throttle is not too high - skips checks if arming from GCS in Guided
if (!(arming_from_gcs && control_mode == GUIDED)) {
// above top of deadband is too always high
if (channel_throttle->control_in > get_takeoff_trigger_throttle()) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Collective too high"));
#else
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Throttle too high"));
#endif
}
return false;
}
// in manual modes throttle must be at zero
if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && channel_throttle->control_in > 0) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Collective too high"));
#else
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Throttle too high"));
#endif
}
return false;
}
}
}
// check if safety switch has been pushed
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Safety Switch"));
}
return false;
}
// if we've gotten this far all is ok
return true;
}
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at MAIN_LOOP_RATE
void Copter::update_land_detector()
{
// land detector can not use the following sensors because they are unreliable during landing
// barometer altitude : ground effect can cause errors larger than 4m
// EKF vertical velocity or altitude : poor barometer and large acceleration from ground impact
// earth frame angle or angle error : landing on an uneven surface will force the airframe to match the ground angle
// gyro output : on uneven surface the airframe may rock back an forth after landing
// range finder : tend to be problematic at very short distances
// input throttle : in slow land the input throttle may be only slightly less than hover
if (!motors.armed()) {
// if disarmed, always landed.
set_land_complete(true);
} else if (ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME
// if rotor speed and collective pitch are high then clear landing flag
if (motors.get_throttle() > get_non_takeoff_throttle() && motors.rotor_runup_complete()) {
#else
// if throttle output is high then clear landing flag
if (motors.get_throttle() > get_non_takeoff_throttle()) {
#endif
set_land_complete(false);
}
} else {
#if FRAME_CONFIG == HELI_FRAME
// check that collective pitch is on lower limit (should be constrained by LAND_COL_MIN)
bool motor_at_lower_limit = motors.limit.throttle_lower;
#else
// check that the average throttle output is near minimum (less than 12.5% hover throttle)
bool motor_at_lower_limit = motors.limit.throttle_lower && motors.is_throttle_mix_min();
#endif
// check that the airframe is not accelerating (not falling or breaking after fast forward flight)
bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX);
if (motor_at_lower_limit && accel_stationary) {
// landed criteria met - increment the counter and check if we've triggered
if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
land_detector_count++;
} else {
set_land_complete(true);
}
} else {
// we've sensed movement up or down so reset land_detector
land_detector_count = 0;
}
}
set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*MAIN_LOOP_RATE));
}
void Copter::set_land_complete(bool b)
{
// if no change, exit immediately
if( ap.land_complete == b )
return;
land_detector_count = 0;
if(b){
Log_Write_Event(DATA_LAND_COMPLETE);
} else {
Log_Write_Event(DATA_NOT_LANDED);
}
ap.land_complete = b;
}
// set land complete maybe flag
void Copter::set_land_complete_maybe(bool b)
{
// if no change, exit immediately
if (ap.land_complete_maybe == b)
return;
if (b) {
Log_Write_Event(DATA_LAND_COMPLETE_MAYBE);
}
ap.land_complete_maybe = b;
}
// update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state
// low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
// has no effect when throttle is above hover throttle
void Copter::update_throttle_thr_mix()
{
#if FRAME_CONFIG != HELI_FRAME
// if disarmed or landed prioritise throttle
if(!motors.armed() || ap.land_complete) {
motors.set_throttle_mix_min();
return;
}
if (mode_has_manual_throttle(control_mode)) {
// manual throttle
if(channel_throttle->get_control_in() <= 0) {
motors.set_throttle_mix_min();
} else {
motors.set_throttle_mix_mid();
}
} else {
// autopilot controlled throttle
// check for aggressive flight requests - requested roll or pitch angle below 15 degrees
const Vector3f angle_target = attitude_control.get_att_target_euler_cd();
bool large_angle_request = (norm(angle_target.x, angle_target.y) > 1500.0f);
// check for large external disturbance - angle error over 30 degrees
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
bool large_angle_error = (norm(angle_error.x, angle_error.y) > 3000.0f);
// check for large acceleration - falling or high turbulence
Vector3f accel_ef = ahrs.get_accel_ef_blended();
accel_ef.z += GRAVITY_MSS;
bool accel_moving = (accel_ef.length() > 3.0f);
// check for requested decent
bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f;
if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
motors.set_throttle_mix_max();
} else {
motors.set_throttle_mix_min();
}
}
#endif
}
// mode_allows_arming - returns true if vehicle can be armed in the specified mode
// arming_from_gcs should be set to true if the arming request comes from the ground station
bool Copter::mode_allows_arming(uint8_t mode, bool arming_from_gcs) {
if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) {
return true;
}
return false;
}
// set_mode - change flight mode and perform any necessary initialisation
// optional force parameter used to force the flight mode change (used only first time mode is set)
// returns true if mode was successfully set
// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
{
// boolean to record if flight mode could be set
bool success = false;
bool ignore_checks = !motors->armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
// return immediately if we are already in the desired mode
if (mode == control_mode) {
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
control_mode_reason = reason;
return true;
}
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter a non-manual throttle mode if the
// rotor runup is not complete
if (!ignore_checks && !mode_has_manual_throttle(mode) && !motors->rotor_runup_complete()){
goto failed;
}
#endif
switch (mode) {
case ACRO:
#if FRAME_CONFIG == HELI_FRAME
success = heli_acro_init(ignore_checks);
#else
success = acro_init(ignore_checks);
#endif
break;
case STABILIZE:
#if FRAME_CONFIG == HELI_FRAME
success = heli_stabilize_init(ignore_checks);
#else
success = stabilize_init(ignore_checks);
#endif
break;
case ALT_HOLD:
success = althold_init(ignore_checks);
break;
case AUTO:
success = auto_init(ignore_checks);
break;
case CIRCLE:
success = circle_init(ignore_checks);
break;
case LOITER:
success = loiter_init(ignore_checks);
break;
case GUIDED:
success = guided_init(ignore_checks);
break;
case LAND:
success = land_init(ignore_checks);
break;
case RTL:
success = rtl_init(ignore_checks);
break;
case DRIFT:
success = drift_init(ignore_checks);
break;
case SPORT:
success = sport_init(ignore_checks);
break;
case FLIP:
success = flip_init(ignore_checks);
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
success = autotune_init(ignore_checks);
break;
#endif
#if POSHOLD_ENABLED == ENABLED
case POSHOLD:
success = poshold_init(ignore_checks);
break;
#endif
case BRAKE:
success = brake_init(ignore_checks);
break;
case THROW:
success = throw_init(ignore_checks);
break;
case AVOID_ADSB:
success = avoid_adsb_init(ignore_checks);
break;
case GUIDED_NOGPS:
success = guided_nogps_init(ignore_checks);
break;
case SMART_RTL:
success = smart_rtl_init(ignore_checks);
break;
default:
success = false;
break;
}
#if FRAME_CONFIG == HELI_FRAME
failed:
#endif
// update flight mode
if (success) {
// perform any cleanup required by previous flight mode
exit_mode(control_mode, mode);
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
control_mode = mode;
control_mode_reason = reason;
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
adsb.set_is_auto_mode((mode == AUTO) || (mode == RTL) || (mode == GUIDED));
#if AC_FENCE == ENABLED
// pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
// this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
// but it should be harmless to disable the fence temporarily in these situations as well
fence.manual_recovery_start();
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry.update_control_mode(control_mode);
#endif
} else {
// Log error that we failed to enter desired flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
}
// update notify object
if (success) {
notify_flight_mode(control_mode);
}
// return success or failure
return success;
}
// arm_checks - perform final checks before arming
// always called just before arming. Return true if ok to arm
// has side-effect that logging is started
bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
{
#if LOGGING_ENABLED == ENABLED
// start dataflash
start_logging();
#endif
// check accels and gyro are healthy
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
//check if accelerometers have calibrated and require reboot
if (ins.accel_cal_requires_reboot()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accelerometers calibrated requires reboot");
}
return false;
}
if (!ins.get_accel_health_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Accelerometers not healthy");
}
return false;
}
if (!ins.get_gyro_health_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Gyros not healthy");
}
return false;
}
// get ekf attitude (if bad, it's usually the gyro biases)
if (!pre_arm_ekf_attitude_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: gyros still settling");
}
return false;
}
}
// always check if inertial nav has started and is ready
if (!ahrs.healthy()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Waiting for Nav Checks");
}
return false;
}
if (compass.is_calibrating()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass calibration running");
}
return false;
}
//check if compass has calibrated and requires reboot
if (compass.compass_cal_requires_reboot()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass calibrated requires reboot");
}
return false;
}
// always check if the current mode allows arming
if (!mode_allows_arming(control_mode, arming_from_gcs)) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Mode not armable");
}
return false;
}
// always check gps
if (!pre_arm_gps_checks(display_failure)) {
return false;
}
// if we are using motor interlock switch and it's enabled, fail to arm
// skip check in Throw mode which takes control of the motor interlock
if (ap.using_interlock && motors.get_interlock()) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Interlock Enabled");
return false;
}
// if we are not using Emergency Stop switch option, force Estop false to ensure motors
// can run normally
if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){
set_motor_emergency_stop(false);
// if we are using motor Estop switch, it must not be in Estop position
} else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped");
return false;
}
// succeed if arming checks are disabled
if (g.arming_check == ARMING_CHECK_NONE) {
return true;
}
// baro checks
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
// baro health check
if (!barometer.all_healthy()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Barometer not healthy");
}
return false;
}
// Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
// Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
// that may differ from the baro height due to baro drift.
nav_filter_status filt_status = inertial_nav.get_filter_status();
bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
if (using_baro_ref && (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Altitude disparity");
}
return false;
}
}
#if AC_FENCE == ENABLED
// check vehicle is within fence
if (!fence.pre_arm_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: check fence");
}
return false;
}
#endif
// check lean angle
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > aparm.angle_max) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Leaning");
}
return false;
}
}
// check battery voltage
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Check Battery");
}
return false;
}
}
// check for missing terrain data
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) {
if (!pre_arm_terrain_check(display_failure)) {
return false;
}
}
// check adsb
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) {
if (failsafe.adsb) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ADSB threat detected");
}
return false;
}
}
// check throttle
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) {
// check throttle is not too low - must be above failsafe throttle
if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->get_radio_in() < g.failsafe_throttle_value) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective below Failsafe");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle below Failsafe");
#endif
}
return false;
}
// check throttle is not too high - skips checks if arming from GCS in Guided
if (!(arming_from_gcs && (control_mode == GUIDED || control_mode == GUIDED_NOGPS))) {
// above top of deadband is too always high
if (get_pilot_desired_climb_rate(channel_throttle->get_control_in()) > 0.0f) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high");
#endif
}
return false;
}
// in manual modes throttle must be at zero
if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && channel_throttle->get_control_in() > 0) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high");
#endif
}
return false;
}
}
}
// check if safety switch has been pushed
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Safety Switch");
}
return false;
}
// if we've gotten this far all is ok
return true;
}
void Copter::update_ground_effect_detector(void)
{
if(!motors.armed()) {
// disarmed - disable ground effect and return
gndeffect_state.takeoff_expected = false;
gndeffect_state.touchdown_expected = false;
ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
return;
}
// variable initialization
uint32_t tnow_ms = millis();
float xy_des_speed_cms = 0.0f;
float xy_speed_cms = 0.0f;
float des_climb_rate_cms = pos_control.get_desired_velocity().z;
if (pos_control.is_active_xy()) {
Vector3f vel_target = pos_control.get_vel_target();
vel_target.z = 0.0f;
xy_des_speed_cms = vel_target.length();
}
if (position_ok() || optflow_position_ok()) {
Vector3f vel = inertial_nav.get_velocity();
vel.z = 0.0f;
xy_speed_cms = vel.length();
}
// takeoff logic
// if we are armed and haven't yet taken off
if (motors.armed() && ap.land_complete && !gndeffect_state.takeoff_expected) {
gndeffect_state.takeoff_expected = true;
}
// if we aren't taking off yet, reset the takeoff timer, altitude and complete flag
bool throttle_up = mode_has_manual_throttle(control_mode) && g.rc_3.control_in > 0;
if (!throttle_up && ap.land_complete) {
gndeffect_state.takeoff_time_ms = tnow_ms;
gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude();
}
// if we are in takeoff_expected and we meet the conditions for having taken off
// end the takeoff_expected state
if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_altitude()-gndeffect_state.takeoff_alt_cm > 50.0f)) {
gndeffect_state.takeoff_expected = false;
}
// landing logic
Vector3f angle_target_rad = attitude_control.get_att_target_euler_cd() * radians(0.01f);
bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f));
bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f;
bool xy_speed_demand_low = pos_control.is_active_xy() && xy_des_speed_cms <= 125.0f;
bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control.is_active_xy()) || (control_mode == ALT_HOLD && small_angle_request);
bool descent_demanded = pos_control.is_active_z() && des_climb_rate_cms < 0.0f;
bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f;
bool z_speed_low = abs(inertial_nav.get_velocity_z()) <= 60.0f;
bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded));
gndeffect_state.touchdown_expected = slow_horizontal && slow_descent;
// Prepare the EKF for ground effect if either takeoff or touchdown is expected.
ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
}
