Sub: move Arming functions into AP_Arming file

ArduSub/AP_Arming_Sub.cpp

@@ -43,3 +43,129 @@ bool AP_Arming_Sub::ins_checks(bool display_failure)
 
     return true;
 }
+
+bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks)
+{
+    static bool in_arm_motors = false;
+
+    // exit immediately if already in this function
+    if (in_arm_motors) {
+        return false;
+    }
+
+    in_arm_motors = true;
+
+    if (!AP_Arming::arm(method, do_arming_checks)) {
+        AP_Notify::events.arming_failed = true;
+        in_arm_motors = false;
+        return false;
+    }
+
+    // let logger know that we're armed (it may open logs e.g.)
+    AP::logger().set_vehicle_armed(true);
+
+    // disable cpu failsafe because initialising everything takes a while
+    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++) {
+        AP::notify().update();
+    }
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    gcs().send_text(MAV_SEVERITY_INFO, "Arming motors");
+#endif
+
+    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)
+
+        // Always use absolute altitude for ROV
+        // ahrs.resetHeightDatum();
+        // 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 (!sub.set_home_to_current_location(false)) {
+            // ignore this failure
+        }
+    }
+
+    // enable gps velocity based centrefugal force compensation
+    ahrs.set_correct_centrifugal(true);
+    hal.util->set_soft_armed(true);
+
+    // enable output to motors
+    sub.enable_motor_output();
+
+    // finally actually arm the motors
+    sub.motors.armed(true);
+
+    AP::logger().Write_Event(DATA_ARMED);
+
+    // log flight mode in case it was changed while vehicle was disarmed
+    AP::logger().Write_Mode(sub.control_mode, sub.control_mode_reason);
+
+    // reenable failsafe
+    sub.mainloop_failsafe_enable();
+
+    // perf monitor ignores delay due to arming
+    AP::scheduler().perf_info.ignore_this_loop();
+
+    // flag exiting this function
+    in_arm_motors = false;
+
+    // return success
+    return true;
+}
+
+bool AP_Arming_Sub::disarm()
+{
+    // return immediately if we are already disarmed
+    if (!sub.motors.armed()) {
+        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() && 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)) {
+                AP::compass().set_and_save_offsets(i, magOffsets);
+            }
+        }
+    }
+
+    AP::logger().Write_Event(DATA_DISARMED);
+
+    // send disarm command to motors
+    sub.motors.armed(false);
+
+    // reset the mission
+    sub.mission.reset();
+
+    AP::logger().set_vehicle_armed(false);
+
+    // disable gps velocity based centrefugal force compensation
+    ahrs.set_correct_centrifugal(false);
+    hal.util->set_soft_armed(false);
+
+    // clear input holds
+    sub.clear_input_hold();
+
+    return true;
+}

ArduSub/motors.cpp

@@ -6,132 +6,6 @@ void Sub::enable_motor_output()
     motors.output_min();
 }
 
-bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks)
-{
-    static bool in_arm_motors = false;
-
-    // exit immediately if already in this function
-    if (in_arm_motors) {
-        return false;
-    }
-
-    in_arm_motors = true;
-
-    if (!AP_Arming::arm(method, do_arming_checks)) {
-        AP_Notify::events.arming_failed = true;
-        in_arm_motors = false;
-        return false;
-    }
-
-    // let logger know that we're armed (it may open logs e.g.)
-    AP::logger().set_vehicle_armed(true);
-
-    // disable cpu failsafe because initialising everything takes a while
-    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++) {
-        AP::notify().update();
-    }
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    gcs().send_text(MAV_SEVERITY_INFO, "Arming motors");
-#endif
-
-    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)
-
-        // Always use absolute altitude for ROV
-        // ahrs.resetHeightDatum();
-        // 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 (!sub.set_home_to_current_location(false)) {
-            // ignore this failure
-        }
-    }
-	
-    // enable gps velocity based centrefugal force compensation
-    ahrs.set_correct_centrifugal(true);
-    hal.util->set_soft_armed(true);
-
-    // enable output to motors
-    sub.enable_motor_output();
-
-    // finally actually arm the motors
-    sub.motors.armed(true);
-
-    AP::logger().Write_Event(DATA_ARMED);
-
-    // log flight mode in case it was changed while vehicle was disarmed
-    AP::logger().Write_Mode(sub.control_mode, sub.control_mode_reason);
-
-    // reenable failsafe
-    sub.mainloop_failsafe_enable();
-
-    // perf monitor ignores delay due to arming
-    AP::scheduler().perf_info.ignore_this_loop();
-
-    // flag exiting this function
-    in_arm_motors = false;
-
-    // return success
-    return true;
-}
-
-bool AP_Arming_Sub::disarm()
-{
-    // return immediately if we are already disarmed
-    if (!sub.motors.armed()) {
-        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() && 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)) {
-                AP::compass().set_and_save_offsets(i, magOffsets);
-            }
-        }
-    }
-
-    AP::logger().Write_Event(DATA_DISARMED);
-
-    // send disarm command to motors
-    sub.motors.armed(false);
-
-    // reset the mission
-    sub.mission.reset();
-
-    AP::logger().set_vehicle_armed(false);
-
-    // disable gps velocity based centrefugal force compensation
-    ahrs.set_correct_centrifugal(false);
-    hal.util->set_soft_armed(false);
-
-    // clear input holds
-    sub.clear_input_hold();
-
-    return true;
-}
-
 // motors_output - send output to motors library which will adjust and send to ESCs and servos
 void Sub::motors_output()
 {