diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index a13abfea1a..822ee8321d 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -12,20 +12,19 @@ void AP_Arming_Copter::update(void) pre_arm_display_counter = 0; } - set_pre_arm_check(pre_arm_checks(display_fail)); + pre_arm_checks(display_fail); } -// performs pre-arm checks and arming checks -bool AP_Arming_Copter::all_checks_passing(AP_Arming::Method method) +bool AP_Arming_Copter::pre_arm_checks(bool display_failure) { - set_pre_arm_check(pre_arm_checks(true)); - - return copter.ap.pre_arm_check && arm_checks(method); + const bool passed = run_pre_arm_checks(display_failure); + set_pre_arm_check(passed); + return passed; } // perform pre-arm checks // return true if the checks pass successfully -bool AP_Arming_Copter::pre_arm_checks(bool display_failure) +bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure) { // exit immediately if already armed if (copter.motors->armed()) { diff --git a/ArduCopter/AP_Arming.h b/ArduCopter/AP_Arming.h index 18d9229f12..9f959e7143 100644 --- a/ArduCopter/AP_Arming.h +++ b/ArduCopter/AP_Arming.h @@ -20,10 +20,12 @@ public: AP_Arming_Copter &operator=(const AP_Arming_Copter&) = delete; void update(void); - bool all_checks_passing(AP_Arming::Method method); bool rc_calibration_checks(bool display_failure) override; + bool disarm() override; + bool arm(AP_Arming::Method method, bool do_arming_checks=true) override; + protected: bool pre_arm_checks(bool display_failure) override; @@ -48,4 +50,8 @@ protected: private: + // actually contains the pre-arm checks. This is wrapped so that + // we can store away success/failure of the checks. + bool run_pre_arm_checks(bool display_failure); + }; diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 6c32189c60..ec66838d0a 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -752,8 +752,6 @@ private: // motors.cpp void arm_motors_check(); void auto_disarm_check(); - bool init_arm_motors(AP_Arming::Method method, bool do_arming_checks=true); - void init_disarm_motors(); void motors_output(); void lost_vehicle_check(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index d2da162a39..a2bd0046be 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -606,6 +606,11 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t return GCS_MAVLINK::handle_command_mount(packet); } +bool GCS_MAVLINK_Copter::allow_disarm() const +{ + return copter.ap.land_complete; +} + MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_long_t &packet) { switch(packet.command) { @@ -699,26 +704,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ return MAV_RESULT_FAILED; #endif - case MAV_CMD_COMPONENT_ARM_DISARM: - if (is_equal(packet.param1,1.0f)) { - // attempt to arm and return success or failure - const bool do_arming_checks = !is_equal(packet.param2,magic_force_arm_value); - if (copter.init_arm_motors(AP_Arming::Method::MAVLINK, do_arming_checks)) { - return MAV_RESULT_ACCEPTED; - } - } else if (is_zero(packet.param1)) { - if (copter.ap.land_complete || is_equal(packet.param2,magic_force_disarm_value)) { - // force disarming by setting param2 = 21196 is deprecated - copter.init_disarm_motors(); - return MAV_RESULT_ACCEPTED; - } else { - return MAV_RESULT_FAILED; - } - } else { - return MAV_RESULT_UNSUPPORTED; - } - return MAV_RESULT_FAILED; - #if PARACHUTE == ENABLED case MAV_CMD_DO_PARACHUTE: // configure or release parachute @@ -819,7 +804,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ if (!copter.motors->armed()) { // if disarmed, arm motors - copter.init_arm_motors(AP_Arming::Method::MAVLINK); + copter.arming.arm(AP_Arming::Method::MAVLINK); } else if (copter.ap.land_complete) { // if armed and landed, takeoff if (copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) { @@ -841,7 +826,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ if (copter.motors->armed()) { if (copter.ap.land_complete) { // if landed, disarm motors - copter.init_disarm_motors(); + copter.arming.disarm(); } else { // assume that shots modes are all done in guided. // NOTE: this may need to change if we add a non-guided shot mode @@ -1309,7 +1294,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_l if (GCS_MAVLINK::handle_flight_termination(packet) != MAV_RESULT_ACCEPTED) { #endif if (packet.param1 > 0.5f) { - copter.init_disarm_motors(); + copter.arming.disarm(); result = MAV_RESULT_ACCEPTED; } #if ADVANCED_FAILSAFE == ENABLED diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index d298b69486..e0af392e99 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -43,6 +43,8 @@ protected: virtual MAV_VTOL_STATE vtol_state() const override { return MAV_VTOL_STATE_MC; }; virtual MAV_LANDED_STATE landed_state() const override; + bool allow_disarm() const override; + private: void handleMessage(mavlink_message_t * msg) override; diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 1fce0831db..9a2db28d3e 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -420,7 +420,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw // arm or disarm the vehicle switch (ch_flag) { case HIGH: - copter.init_arm_motors(AP_Arming::Method::AUXSWITCH); + copter.arming.arm(AP_Arming::Method::AUXSWITCH); // remember that we are using an arming switch, for use by set_throttle_zero_flag copter.ap.armed_with_switch = true; break; @@ -428,7 +428,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw // nothing break; case LOW: - copter.init_disarm_motors(); + copter.arming.disarm(); break; } break; diff --git a/ArduCopter/afs_copter.cpp b/ArduCopter/afs_copter.cpp index c36ece99f0..44c325df45 100644 --- a/ArduCopter/afs_copter.cpp +++ b/ArduCopter/afs_copter.cpp @@ -25,7 +25,7 @@ void AP_AdvancedFailsafe_Copter::terminate_vehicle(void) copter.motors->output(); // disarm as well - copter.init_disarm_motors(); + copter.arming.disarm(); // and set all aux channels SRV_Channels::set_output_limit(SRV_Channel::k_heli_rsc, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); diff --git a/ArduCopter/avoidance_adsb.cpp b/ArduCopter/avoidance_adsb.cpp index 73f6ba7dd7..7eaffe81be 100644 --- a/ArduCopter/avoidance_adsb.cpp +++ b/ArduCopter/avoidance_adsb.cpp @@ -34,7 +34,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O // if landed and we will take some kind of action, just disarm if ((actual_action > MAV_COLLISION_ACTION_REPORT) && copter.should_disarm_on_failsafe()) { - copter.init_disarm_motors(); + copter.arming.disarm(); actual_action = MAV_COLLISION_ACTION_NONE; } else { diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 22578320f2..e51e930982 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -53,7 +53,7 @@ void Copter::crash_check() // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming"); // disarm motors - init_disarm_motors(); + copter.arming.disarm(); } } @@ -210,7 +210,7 @@ void Copter::parachute_check() void Copter::parachute_release() { // disarm motors - init_disarm_motors(); + arming.disarm(); // release parachute parachute.release(); diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 810375b54d..af856c8a7c 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -12,7 +12,7 @@ void Copter::failsafe_radio_on_event() } if (should_disarm_on_failsafe()) { - init_disarm_motors(); + arming.disarm(); } else { if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) { // continue mission @@ -55,7 +55,7 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action) // failsafe check if (should_disarm_on_failsafe()) { - init_disarm_motors(); + arming.disarm(); } else { switch ((Failsafe_Action)action) { case Failsafe_Action_None: @@ -78,7 +78,7 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action) snprintf(battery_type_str, 17, "%s battery", type_str); g2.afs.gcs_terminate(true, battery_type_str); #else - init_disarm_motors(); + arming.disarm(); #endif } } @@ -122,7 +122,7 @@ void Copter::failsafe_gcs_check() RC_Channels::clear_overrides(); if (should_disarm_on_failsafe()) { - init_disarm_motors(); + arming.disarm(); } else { if (control_mode == AUTO && g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) { // continue mission @@ -190,7 +190,7 @@ void Copter::failsafe_terrain_on_event() AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED); if (should_disarm_on_failsafe()) { - init_disarm_motors(); + arming.disarm(); #if MODE_RTL_ENABLED == ENABLED } else if (control_mode == RTL) { mode_rtl.restart_without_terrain(); diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index f7e0ebb886..9bd4c66712 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -30,7 +30,7 @@ void Copter::fence_check() // 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 || (flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){ - init_disarm_motors(); + arming.disarm(); } else { diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 52ca11dec1..a3dbe0f282 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -117,7 +117,7 @@ void Copter::set_land_complete(bool b) const bool mode_disarms_on_land = flightmode->allows_arming(false) && !flightmode->has_manual_throttle(); if (ap.land_complete && motors->armed() && disarm_on_land_configured && mode_disarms_on_land) { - init_disarm_motors(); + arming.disarm(); } } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 5c48685b3d..85ae014669 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -537,7 +537,7 @@ void Copter::ModeAuto::exit_mission() } } else { // if we've landed it's safe to disarm - copter.init_disarm_motors(); + copter.arming.disarm(); } } diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index eb4c34065e..d4dbb7b957 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -55,7 +55,7 @@ void Copter::ModeLand::gps_run() { // disarm when the landing detector says we've landed if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { - copter.init_disarm_motors(); + copter.arming.disarm(); } // Land State Machine Determination @@ -109,7 +109,7 @@ void Copter::ModeLand::nogps_run() // disarm when the landing detector says we've landed if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { - copter.init_disarm_motors(); + copter.arming.disarm(); } // Land State Machine Determination diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index f5dafb5434..9c6d23832d 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -378,7 +378,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land) // disarm when the landing detector says we've landed if (disarm_on_land && ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { - copter.init_disarm_motors(); + copter.arming.disarm(); } // if not armed set throttle to zero and exit immediately diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index c8114d70f3..5ed77b1987 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -45,7 +45,7 @@ void Copter::arm_motors_check() // arm the motors and configure for flight if (arming_counter == ARM_DELAY && !motors->armed()) { // reset arming counter if arming fail - if (!init_arm_motors(AP_Arming::Method::RUDDER)) { + if (!arming.arm(AP_Arming::Method::RUDDER)) { arming_counter = 0; } } @@ -71,7 +71,7 @@ void Copter::arm_motors_check() // disarm the motors if (arming_counter == DISARM_DELAY && motors->armed()) { - init_disarm_motors(); + arming.disarm(); } // Yaw is centered so reset arming counter @@ -124,14 +124,12 @@ void Copter::auto_disarm_check() // disarm once timer expires if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) { - init_disarm_motors(); + arming.disarm(); auto_disarm_begin = tnow_ms; } } -// 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(const AP_Arming::Method method, const bool do_arming_checks) +bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_checks) { static bool in_arm_motors = false; @@ -142,13 +140,12 @@ bool Copter::init_arm_motors(const AP_Arming::Method method, const bool do_armin in_arm_motors = true; // return true if already armed - if (motors->armed()) { + if (copter.motors->armed()) { in_arm_motors = false; return true; } - // run pre-arm-checks and display failures - if (do_arming_checks && !arming.all_checks_passing(method)) { + if (!AP_Arming::arm(method, do_arming_checks)) { AP_Notify::events.arming_failed = true; in_arm_motors = false; return false; @@ -158,13 +155,13 @@ bool Copter::init_arm_motors(const AP_Arming::Method method, const bool do_armin AP::logger().set_vehicle_armed(true); // disable cpu failsafe because initialising everything takes a while - failsafe_disable(); + copter.failsafe_disable(); // 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(); + AP::notify().update(); } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -173,31 +170,33 @@ bool Copter::init_arm_motors(const AP_Arming::Method method, const bool do_armin // Remember Orientation // -------------------- - init_simple_bearing(); + copter.init_simple_bearing(); - initial_armed_bearing = ahrs.yaw_sensor; + AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); + + copter.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); + AP::logger().Write_Event(DATA_EKF_ALT_RESET); // we have reset height, so arming height is zero - arming_altitude_m = 0; + copter.arming_altitude_m = 0; } else if (!ahrs.home_is_locked()) { // Reset home position if it has already been set before (but not locked) - if (!set_home_to_current_location(false)) { + if (!copter.set_home_to_current_location(false)) { // ignore failure } // remember the height when we armed - arming_altitude_m = inertial_nav.get_altitude() * 0.01; + copter.arming_altitude_m = copter.inertial_nav.get_altitude() * 0.01; } - update_super_simple_bearing(false); + copter.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()); + copter.g2.smart_rtl.set_home(copter.position_ok()); #endif // enable gps velocity based centrefugal force compensation @@ -206,55 +205,62 @@ bool Copter::init_arm_motors(const AP_Arming::Method method, const bool do_armin #if SPRAYER_ENABLED == ENABLED // turn off sprayer's test if on - sprayer.test_pump(false); + copter.sprayer.test_pump(false); #endif // enable output to motors - enable_motor_output(); + copter.enable_motor_output(); // finally actually arm the motors - motors->armed(true); + copter.motors->armed(true); - Log_Write_Event(DATA_ARMED); + AP::logger().Write_Event(DATA_ARMED); // log flight mode in case it was changed while vehicle was disarmed - logger.Write_Mode(control_mode, control_mode_reason); + AP::logger().Write_Mode(copter.control_mode, copter.control_mode_reason); // re-enable failsafe - failsafe_enable(); + copter.failsafe_enable(); // perf monitor ignores delay due to arming - scheduler.perf_info.ignore_this_loop(); + AP::scheduler().perf_info.ignore_this_loop(); // flag exiting this function in_arm_motors = false; // Log time stamp of arming event - arm_time_ms = millis(); + copter.arm_time_ms = millis(); // Start the arming delay - ap.in_arming_delay = true; + copter.ap.in_arming_delay = true; // assumed armed without a arming, switch. Overridden in switches.cpp - ap.armed_with_switch = false; - + copter.ap.armed_with_switch = false; + // return success return true; } -// init_disarm_motors - disarm motors -void Copter::init_disarm_motors() +// arming.disarm - disarm motors +bool AP_Arming_Copter::disarm() { // return immediately if we are already disarmed - if (!motors->armed()) { - return; + if (!copter.motors->armed()) { + return true; + } + + if (!AP_Arming::disarm()) { + return false; } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors"); #endif + AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); + // save compass offsets learned by the EKF if enabled + Compass &compass = AP::compass(); if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) { for(uint8_t i=0; iarmed(false); + copter.motors->armed(false); #if MODE_AUTO_ENABLED == ENABLED // reset the mission - mode_auto.mission.reset(); + copter.mode_auto.mission.reset(); #endif AP::logger().set_vehicle_armed(false); @@ -293,7 +299,9 @@ void Copter::init_disarm_motors() ahrs.set_correct_centrifugal(false); hal.util->set_soft_armed(false); - ap.in_arming_delay = false; + copter.ap.in_arming_delay = false; + + return true; } // motors_output - send output to motors library which will adjust and send to ESCs and servos diff --git a/ArduCopter/toy_mode.cpp b/ArduCopter/toy_mode.cpp index 2a3d07eb1c..f9c30a825e 100644 --- a/ArduCopter/toy_mode.cpp +++ b/ArduCopter/toy_mode.cpp @@ -424,7 +424,7 @@ void ToyMode::update() if (throttle_high_counter >= TOY_LAND_ARM_COUNT) { gcs().send_text(MAV_SEVERITY_INFO, "Tmode: throttle arm"); arm_check_compass(); - if (!copter.init_arm_motors(AP_Arming::Method::MAVLINK) && (flags & FLAG_UPGRADE_LOITER) && copter.control_mode == LOITER) { + if (!copter.arming.arm(AP_Arming::Method::MAVLINK) && (flags & FLAG_UPGRADE_LOITER) && copter.control_mode == LOITER) { /* support auto-switching to ALT_HOLD, then upgrade to LOITER once GPS available */ @@ -433,7 +433,7 @@ void ToyMode::update() #if AC_FENCE == ENABLED copter.fence.enable(false); #endif - if (!copter.init_arm_motors(AP_Arming::Method::MAVLINK)) { + if (!copter.arming.arm(AP_Arming::Method::MAVLINK)) { // go back to LOITER gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: ALT_HOLD arm failed"); set_and_remember_mode(LOITER, MODE_REASON_TMODE); @@ -625,7 +625,7 @@ void ToyMode::update() #if AC_FENCE == ENABLED copter.fence.enable(false); #endif - if (copter.init_arm_motors(AP_Arming::Method::MAVLINK)) { + if (copter.arming.arm(AP_Arming::Method::MAVLINK)) { load_test.running = true; gcs().send_text(MAV_SEVERITY_INFO, "Tmode: load_test on"); } else { @@ -803,7 +803,7 @@ void ToyMode::action_arm(void) // we want GPS and checks are passing, arm and enable fence copter.fence.enable(true); #endif - copter.init_arm_motors(AP_Arming::Method::RUDDER); + copter.arming.arm(AP_Arming::Method::RUDDER); if (!copter.motors->armed()) { AP_Notify::events.arming_failed = true; gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: GPS arming failed"); @@ -819,7 +819,7 @@ void ToyMode::action_arm(void) // non-GPS mode copter.fence.enable(false); #endif - copter.init_arm_motors(AP_Arming::Method::RUDDER); + copter.arming.arm(AP_Arming::Method::RUDDER); if (!copter.motors->armed()) { AP_Notify::events.arming_failed = true; gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: non-GPS arming failed");