From b25acb4d22dee571f43eab41ce7195174590d115 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 30 May 2019 09:37:47 +1000 Subject: [PATCH] Sub: move Arming functions into AP_Arming file --- ArduSub/AP_Arming_Sub.cpp | 126 ++++++++++++++++++++++++++++++++++++++ ArduSub/motors.cpp | 126 -------------------------------------- 2 files changed, 126 insertions(+), 126 deletions(-) diff --git a/ArduSub/AP_Arming_Sub.cpp b/ArduSub/AP_Arming_Sub.cpp index 39bebb4a1b..dcf3e56a04 100644 --- a/ArduSub/AP_Arming_Sub.cpp +++ b/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; iset_soft_armed(false); + + // clear input holds + sub.clear_input_hold(); + + return true; +} diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 8fc395a07f..b67f523ecc 100644 --- a/ArduSub/motors.cpp +++ b/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; iset_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() {