// 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); }