//returns true if arming occurred successfully bool AP_Arming::arm(AP_Arming::ArmingMethod method, const bool do_arming_checks) { #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) // Copter should never use this function return false; #else if (armed) { //already armed return false; } //are arming checks disabled? if (!do_arming_checks || checks_to_perform == ARMING_CHECK_NONE) { armed = true; gcs().send_text(MAV_SEVERITY_INFO, "Throttle armed"); return true; } if (pre_arm_checks(true) && arm_checks(method)) { armed = true; gcs().send_text(MAV_SEVERITY_INFO, "Throttle armed"); //TODO: Log motor arming to the dataflash //Can't do this from this class until there is a unified logging library } else { armed = false; } return armed; #endif }
// performs pre-arm checks and arming checks bool Copter::all_arming_checks_passing(bool arming_from_gcs) { if (pre_arm_checks(true)) { set_pre_arm_check(true); } return ap.pre_arm_check && arm_checks(true, arming_from_gcs); }
uint32_t Copter::get_ready_to_arm_mode_mask(void) { uint32_t ret = 0; control_mode_t saved_control_mode = control_mode; for (uint8_t i=0; i < FLIGHT_MODE_MAX; i++) { if (is_valid_flight_mode(i)) { control_mode = (control_mode_t)i; if (pre_arm_checks(false) && arm_checks(false,true)) { ret |= 1UL << control_mode; } } } control_mode = saved_control_mode; return ret; }
// performs pre-arm checks. expects to be called at 1hz. void Copter::update_arming_checks(void) { if (!motors.armed()) { // perform pre-arm checks & display failures every 30 seconds static uint8_t pre_arm_display_counter = PREARM_DISPLAY_PERIOD/2; pre_arm_display_counter++; bool display_fail = false; if (pre_arm_display_counter >= PREARM_DISPLAY_PERIOD) { display_fail = true; pre_arm_display_counter = 0; } if (pre_arm_checks(display_fail)) { set_pre_arm_check(true); } if (display_fail && ap.pre_arm_check) { // pre-arm checks have passed, run arming checks in order to display failures // this is a HACK that allows the GCS to display arming failures without attempting to arm arm_checks(true,true); } } }
// init_arm_motors - performs arming process including initialisation of barometer and gyros // returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure bool Copter::init_arm_motors(bool arming_from_gcs) { static bool in_arm_motors = false; // exit immediately if already in this function if (in_arm_motors) { return false; } in_arm_motors = true; // run pre-arm-checks and display failures if(!pre_arm_checks(true) || !arm_checks(true, arming_from_gcs)) { AP_Notify::events.arming_failed = true; in_arm_motors = false; return false; } // disable cpu failsafe because initialising everything takes a while failsafe_disable(); // reset battery failsafe set_failsafe_battery(false); // notify that arming will occur (we do this early to give plenty of warning) AP_Notify::flags.armed = true; // call update_notify a few times to ensure the message gets out for (uint8_t i=0; i<=10; i++) { update_notify(); } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs_send_text(MAV_SEVERITY_INFO, "ARMING MOTORS"); #endif // Remember Orientation // -------------------- init_simple_bearing(); initial_armed_bearing = ahrs.yaw_sensor; if (ap.home_state == HOME_UNSET) { // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) ahrs.resetHeightDatum(); Log_Write_Event(DATA_EKF_ALT_RESET); } else if (ap.home_state == HOME_SET_NOT_LOCKED) { // Reset home position if it has already been set before (but not locked) set_home_to_current_location(); } calc_distance_and_bearing(); // enable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(true); hal.util->set_soft_armed(true); #if SPRAYER == ENABLED // turn off sprayer's test if on sprayer.test_pump(false); #endif // short delay to allow reading of rc inputs delay(30); // enable output to motors enable_motor_output(); // finally actually arm the motors motors.armed(true); // log arming to dataflash Log_Write_Event(DATA_ARMED); // log flight mode in case it was changed while vehicle was disarmed DataFlash.Log_Write_Mode(control_mode); // reenable failsafe failsafe_enable(); // perf monitor ignores delay due to arming perf_ignore_this_loop(); // flag exiting this function in_arm_motors = false; // return success return true; }
// init_arm_motors - performs arming process including initialisation of barometer and gyros // returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure bool Copter::init_arm_motors(bool arming_from_gcs) { // arming marker // Flag used to track if we have armed the motors the first time. // This is used to decide if we should run the ground_start routine // which calibrates the IMU static bool did_ground_start = false; static bool in_arm_motors = false; // exit immediately if already in this function if (in_arm_motors) { return false; } in_arm_motors = true; // run pre-arm-checks and display failures if(!pre_arm_checks(true) || !arm_checks(true, arming_from_gcs)) { AP_Notify::events.arming_failed = true; in_arm_motors = false; return false; } // disable cpu failsafe because initialising everything takes a while failsafe_disable(); // reset battery failsafe set_failsafe_battery(false); // notify that arming will occur (we do this early to give plenty of warning) AP_Notify::flags.armed = true; // call update_notify a few times to ensure the message gets out for (uint8_t i=0; i<=10; i++) { update_notify(); } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS")); #endif // Remember Orientation // -------------------- init_simple_bearing(); initial_armed_bearing = ahrs.yaw_sensor; if (ap.home_state == HOME_UNSET) { // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) ahrs.get_NavEKF().resetHeightDatum(); Log_Write_Event(DATA_EKF_ALT_RESET); } else if (ap.home_state == HOME_SET_NOT_LOCKED) { // Reset home position if it has already been set before (but not locked) set_home_to_current_location(); } calc_distance_and_bearing(); if(did_ground_start == false) { startup_ground(true); // final check that gyros calibrated successfully if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) && !ins.gyro_calibrated_ok_all()) { gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Gyro calibration failed")); AP_Notify::flags.armed = false; failsafe_enable(); in_arm_motors = false; return false; } did_ground_start = true; } // check if we are using motor interlock control on an aux switch set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK)); // if we are using motor interlock switch and it's enabled, fail to arm if (ap.using_interlock && motors.get_interlock()){ gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Motor Interlock Enabled")); AP_Notify::flags.armed = false; 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_P(SEVERITY_HIGH,PSTR("Arm: Motor Emergency Stopped")); AP_Notify::flags.armed = false; return false; } // enable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(true); hal.util->set_soft_armed(true); #if SPRAYER == ENABLED // turn off sprayer's test if on sprayer.test_pump(false); #endif // short delay to allow reading of rc inputs delay(30); // enable output to motors enable_motor_output(); // finally actually arm the motors motors.armed(true); // log arming to dataflash Log_Write_Event(DATA_ARMED); // log flight mode in case it was changed while vehicle was disarmed DataFlash.Log_Write_Mode(control_mode); // reenable failsafe failsafe_enable(); // perf monitor ignores delay due to arming perf_ignore_this_loop(); // flag exiting this function in_arm_motors = false; // return success return true; }
// performs pre-arm checks and arming checks bool AP_Arming_Copter::all_checks_passing(AP_Arming::Method method) { set_pre_arm_check(pre_arm_checks(true)); return copter.ap.pre_arm_check && arm_checks(true, method); }