From 8566a17a1d7b5ad6aff498a5620d45d9f5b8b6fe Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 4 May 2019 10:14:56 +1000 Subject: [PATCH] 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 | 3 +++ ArduSub/GCS_Mavlink.cpp | 18 +------------- ArduSub/Sub.h | 2 -- ArduSub/failsafe.cpp | 18 +++++++------- ArduSub/joystick.cpp | 8 +++--- ArduSub/motors.cpp | 55 +++++++++++++++++++++++------------------ 6 files changed, 48 insertions(+), 56 deletions(-) diff --git a/ArduSub/AP_Arming_Sub.h b/ArduSub/AP_Arming_Sub.h index f2d17fc2fb..12a57cbb5d 100644 --- a/ArduSub/AP_Arming_Sub.h +++ b/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; }; diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 4a07f0d296..466611ff33 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/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; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index a00f1fa619..71624a30dc 100644 --- a/ArduSub/Sub.h +++ b/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); diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp index c1efa139dd..1453acc05b 100644 --- a/ArduSub/failsafe.cpp +++ b/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(); } } diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index dd523d9ce7..5ef92e5532 100644 --- a/ArduSub/joystick.cpp +++ b/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: diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 29094ac733..8fc395a07f 100644 --- a/ArduSub/motors.cpp +++ b/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 -// 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) +bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks) { 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 -void Sub::init_disarm_motors() +bool AP_Arming_Sub::disarm() { // return immediately if we are already disarmed - if (!motors.armed()) { - return; + 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() && compass.get_learn_type() == Compass::LEARN_EKF) { + if (ahrs.use_compass() && AP::compass().get_learn_type() == Compass::LEARN_EKF) { for (uint8_t i=0; iset_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