Copter: use AP_Arming methods to arm and disarm vehicle

Really just changing the namespace of init_arm_motors

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()) {

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

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();
 

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

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;

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;

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

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 {
 

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();

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();

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 {
 

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

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

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

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

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; i<COMPASS_MAX_INSTANCES; i++) {
             Vector3f magOffsets;
@@ -266,25 +272,25 @@ void Copter::init_disarm_motors()
 
 #if AUTOTUNE_ENABLED == ENABLED
     // save auto tuned parameters
-    if (control_mode == AUTOTUNE) {;
-        mode_autotune.save_tuning_gains();
+    if (copter.flightmode == &copter.mode_autotune) {
+        copter.mode_autotune.save_tuning_gains();
     } else {
-        mode_autotune.reset();
+        copter.mode_autotune.reset();
     }
 #endif
 
     // we are not in the air
-    set_land_complete(true);
-    set_land_complete_maybe(true);
+    copter.set_land_complete(true);
+    copter.set_land_complete_maybe(true);
 
-    Log_Write_Event(DATA_DISARMED);
+    AP::logger().Write_Event(DATA_DISARMED);
 
     // send disarm command to motors
-    motors->armed(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

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");