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(); } }
// 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; }
// 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.limit.throttle_lower && 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 && attitude_control.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); // check that vertical speed is within 1m/s of zero bool descent_rate_low = fabsf(inertial_nav.get_velocity_z()) < 100; // if we have a healthy rangefinder only allow landing detection below 2 meters bool rangefinder_check = (!rangefinder_alt_ok() || rangefinder_state.alt_cm_filt.get() < LAND_RANGEFINDER_MIN_ALT_CM); if (motor_at_lower_limit && accel_stationary && descent_rate_low && rangefinder_check) { // landed criteria met - increment the counter and check if we've triggered if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*scheduler.get_loop_rate_hz()) { 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*scheduler.get_loop_rate_hz())); } // set land_complete flag and disarm motors if disarm-on-land is configured 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; // 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(); } } // 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) { attitude_control.set_throttle_mix_min(); return; } if (mode_has_manual_throttle(control_mode)) { // manual throttle if(channel_throttle->get_control_in() <= 0) { attitude_control.set_throttle_mix_min(); } else { attitude_control.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) > LAND_CHECK_LARGE_ANGLE_CD); // check for large external disturbance - angle error over 30 degrees const float angle_error = attitude_control.get_att_error_angle_deg(); bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG); // 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() > LAND_CHECK_ACCEL_MOVING); // 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) { attitude_control.set_throttle_mix_max(); } else { attitude_control.set_throttle_mix_min(); } } #endif }