// 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; // return true if already armed if (motors->armed()) { in_arm_motors = false; return true; } // run pre-arm-checks and display failures if (!arming.all_checks_passing(arming_from_gcs)) { AP_Notify::events.arming_failed = true; in_arm_motors = false; return false; } // let dataflash know that we're armed (it may open logs e.g.) DataFlash_Class::instance()->set_vehicle_armed(true); // 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 notify update a few times to ensure the message gets out for (uint8_t i=0; i<=10; i++) { notify.update(); } #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 (!ahrs.home_is_set()) { // 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); // we have reset height, so arming height is zero arming_altitude_m = 0; } else if (ahrs.home_status() == HOME_SET_NOT_LOCKED) { // Reset home position if it has already been set before (but not locked) set_home_to_current_location(false); // remember the height when we armed arming_altitude_m = inertial_nav.get_altitude() * 0.01; } update_super_simple_bearing(false); // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point #if MODE_SMARTRTL_ENABLED == ENABLED g2.smart_rtl.set_home(position_ok()); #endif // 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 // 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, control_mode_reason); // reenable failsafe failsafe_enable(); // perf monitor ignores delay due to arming scheduler.perf_info.ignore_this_loop(); // flag exiting this function in_arm_motors = false; // Log time stamp of arming event arm_time_ms = millis(); // Start the arming delay ap.in_arming_delay = true; // 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) { 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; }