Sub: move arming-related functionality into parent classes

Logging moves up
arming via mavlink moves up
arming via switch moves up

arming switch may be forced off using BRD parameter

ArduSub/AP_Arming_Sub.h

@@ -14,6 +14,9 @@ public:
     bool rc_calibration_checks(bool display_failure) override;
     bool pre_arm_checks(bool display_failure) override;
 
+    bool disarm() override;
+    bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
+
 protected:
     bool ins_checks(bool display_failure) override;
 };

ArduSub/GCS_Mavlink.cpp

@@ -511,22 +511,6 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon
         }
         return MAV_RESULT_FAILED;
 
-    case MAV_CMD_COMPONENT_ARM_DISARM:
-        if (is_equal(packet.param1,1.0f)) {
-            // attempt to arm and return success or failure
-            if (sub.init_arm_motors(AP_Arming::Method::MAVLINK)) {
-                return MAV_RESULT_ACCEPTED;
-            }
-        } else if (is_zero(packet.param1))  {
-            // force disarming by setting param2 = 21196 is deprecated
-            // see COMMAND_LONG DO_FLIGHTTERMINATION
-            sub.init_disarm_motors();
-            return MAV_RESULT_ACCEPTED;
-        } else {
-            return MAV_RESULT_UNSUPPORTED;
-        }
-        return MAV_RESULT_FAILED;
-
     case MAV_CMD_DO_MOTOR_TEST:
         // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
         // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
@@ -850,7 +834,7 @@ void Sub::mavlink_delay_cb()
 
 MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long_t &packet) {
     if (packet.param1 > 0.5f) {
-        sub.init_disarm_motors();
+        sub.arming.disarm();
         return MAV_RESULT_ACCEPTED;
     }
     return MAV_RESULT_FAILED;

ArduSub/Sub.h

@@ -588,8 +588,6 @@ private:
     void update_surface_and_bottom_detector();
     void set_surfaced(bool at_surface);
     void set_bottomed(bool at_bottom);
-    bool init_arm_motors(AP_Arming::Method method);
-    void init_disarm_motors();
     void motors_output();
     Vector3f pv_location_to_vector(const Location& loc);
     float pv_alt_above_origin(float alt_above_home_cm);

ArduSub/failsafe.cpp

@@ -92,7 +92,7 @@ void Sub::failsafe_sensors_check()
         // This should always succeed
         if (!set_mode(MANUAL, MODE_REASON_BAD_DEPTH)) {
             // We should never get here
-            init_disarm_motors();
+            arming.disarm();
         }
     }
 }
@@ -146,7 +146,7 @@ void Sub::failsafe_ekf_check()
     }
 
     if (g.fs_ekf_action == FS_EKF_ACTION_DISARM) {
-        init_disarm_motors();
+        arming.disarm();
     }
 }
 
@@ -160,7 +160,7 @@ void Sub::handle_battery_failsafe(const char* type_str, const int8_t action)
             set_mode(SURFACE, MODE_REASON_BATTERY_FAILSAFE);
             break;
         case Failsafe_Action_Disarm:
-            init_disarm_motors();
+            arming.disarm();
             break;
         case Failsafe_Action_Warn:
         case Failsafe_Action_None:
@@ -194,7 +194,7 @@ void Sub::failsafe_pilot_input_check()
     set_neutral_controls();
 
     if(g.failsafe_pilot_input == FS_PILOT_INPUT_DISARM) {
-        init_disarm_motors();
+        arming.disarm();
     }
 #endif
 }
@@ -345,14 +345,14 @@ void Sub::failsafe_gcs_check()
 
     // handle failsafe action
     if (g.failsafe_gcs == FS_GCS_DISARM) {
-        init_disarm_motors();
+        arming.disarm();
     } else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) {
         if (!set_mode(ALT_HOLD, MODE_REASON_GCS_FAILSAFE)) {
-            init_disarm_motors();
+            arming.disarm();
         }
     } else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) {
         if (!set_mode(SURFACE, MODE_REASON_GCS_FAILSAFE)) {
-            init_disarm_motors();
+            arming.disarm();
         }
     }
 }
@@ -411,7 +411,7 @@ void Sub::failsafe_crash_check()
 
     // disarm motors
     if (g.fs_crash_check == FS_CRASH_DISARM) {
-        init_disarm_motors();
+        arming.disarm();
     }
 }
 
@@ -490,6 +490,6 @@ void Sub::failsafe_terrain_act()
 
     case FS_TERRAIN_DISARM:
     default:
-        init_disarm_motors();
+        arming.disarm();
     }
 }

ArduSub/joystick.cpp

@@ -128,16 +128,16 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
     switch (get_button(button)->function(shift)) {
     case JSButton::button_function_t::k_arm_toggle:
         if (motors.armed()) {
-            init_disarm_motors();
+            arming.disarm();
         } else {
-            init_arm_motors(AP_Arming::Method::MAVLINK);
+            arming.arm(AP_Arming::Method::MAVLINK);
         }
         break;
     case JSButton::button_function_t::k_arm:
-        init_arm_motors(AP_Arming::Method::MAVLINK);
+        arming.arm(AP_Arming::Method::MAVLINK);
         break;
     case JSButton::button_function_t::k_disarm:
-        init_disarm_motors();
+        arming.disarm();
         break;
 
     case JSButton::button_function_t::k_mode_manual:

ArduSub/motors.cpp

@@ -6,9 +6,7 @@ void Sub::enable_motor_output()
     motors.output_min();
 }
 
-// init_arm_motors - performs arming process including initialisation of barometer and gyros
+bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks)
-//  returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure
-bool Sub::init_arm_motors(AP_Arming::Method method)
 {
     static bool in_arm_motors = false;
 
@@ -19,7 +17,7 @@ bool Sub::init_arm_motors(AP_Arming::Method method)
 
     in_arm_motors = true;
 
-    if (!arming.pre_arm_checks(true)) {
+    if (!AP_Arming::arm(method, do_arming_checks)) {
         AP_Notify::events.arming_failed = true;
         in_arm_motors = false;
         return false;
@@ -29,20 +27,22 @@ bool Sub::init_arm_motors(AP_Arming::Method method)
     AP::logger().set_vehicle_armed(true);
 
     // disable cpu failsafe because initialising everything takes a while
-    mainloop_failsafe_disable();
+    sub.mainloop_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 CONFIG_HAL_BOARD == HAL_BOARD_SITL
     gcs().send_text(MAV_SEVERITY_INFO, "Arming motors");
 #endif
 
-    initial_armed_bearing = ahrs.yaw_sensor;
+    AP_AHRS &ahrs = AP::ahrs();
+
+    sub.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)
@@ -52,7 +52,7 @@ bool Sub::init_arm_motors(AP_Arming::Method method)
         // Log_Write_Event(DATA_EKF_ALT_RESET);
     } else if (ahrs.home_is_set() && !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 (!sub.set_home_to_current_location(false)) {
             // ignore this failure
         }
     }
@@ -62,21 +62,21 @@ bool Sub::init_arm_motors(AP_Arming::Method method)
     hal.util->set_soft_armed(true);
 
     // enable output to motors
-    enable_motor_output();
+    sub.enable_motor_output();
 
     // finally actually arm the motors
-    motors.armed(true);
+    sub.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(sub.control_mode, sub.control_mode_reason);
 
     // reenable failsafe
-    mainloop_failsafe_enable();
+    sub.mainloop_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;
@@ -85,35 +85,40 @@ bool Sub::init_arm_motors(AP_Arming::Method method)
     return true;
 }
 
-// init_disarm_motors - disarm motors
+bool AP_Arming_Sub::disarm()
-void Sub::init_disarm_motors()
 {
     // return immediately if we are already disarmed
-    if (!motors.armed()) {
+    if (!sub.motors.armed()) {
-        return;
+        return false;
+    }
+
+    if (!AP_Arming::disarm()) {
+        return false;
     }
 
 #if 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
-    if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) {
+    if (ahrs.use_compass() && AP::compass().get_learn_type() == Compass::LEARN_EKF) {
         for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
             Vector3f magOffsets;
             if (ahrs.getMagOffsets(i, magOffsets)) {
-                compass.set_and_save_offsets(i, magOffsets);
+                AP::compass().set_and_save_offsets(i, magOffsets);
             }
         }
     }
 
-    Log_Write_Event(DATA_DISARMED);
+    AP::logger().Write_Event(DATA_DISARMED);
 
     // send disarm command to motors
-    motors.armed(false);
+    sub.motors.armed(false);
 
     // reset the mission
-    mission.reset();
+    sub.mission.reset();
 
     AP::logger().set_vehicle_armed(false);
 
@@ -122,7 +127,9 @@ void Sub::init_disarm_motors()
     hal.util->set_soft_armed(false);
 
     // clear input holds
-    clear_input_hold();
+    sub.clear_input_hold();
+
+    return true;
 }
 
 // motors_output - send output to motors library which will adjust and send to ESCs and servos