From 9851ec1ddb6b3d1fbd88d964308a86bf248535f2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 3 May 2019 22:32:42 +1000 Subject: [PATCH] Rover: move arming-related functionality into parent classes Logging moves up arming via mavlink moves up arming via switch moves up --- APMrover2/APMrover2.cpp | 8 ++++---- APMrover2/AP_Arming.h | 6 ++++++ APMrover2/GCS_Mavlink.cpp | 17 ----------------- APMrover2/Log.cpp | 21 --------------------- APMrover2/RC_Channel.cpp | 9 --------- APMrover2/Rover.h | 5 ----- APMrover2/afs_rover.cpp | 2 +- APMrover2/crash_check.cpp | 4 ++-- APMrover2/defines.h | 3 +-- APMrover2/failsafe.cpp | 4 ++-- APMrover2/motor_test.cpp | 4 ++-- APMrover2/radio.cpp | 4 ++-- APMrover2/system.cpp | 17 +++++++++-------- 13 files changed, 29 insertions(+), 75 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 669354720c..be485c12ec 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -141,17 +141,17 @@ void Rover::loop() G_Dt = scheduler.get_last_loop_time_s(); } -void Rover::update_soft_armed() +void AP_Arming_Rover::update_soft_armed() { - hal.util->set_soft_armed(arming.is_armed() && + hal.util->set_soft_armed(is_armed() && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); - logger.set_vehicle_armed(hal.util->get_soft_armed()); + AP::logger().set_vehicle_armed(hal.util->get_soft_armed()); } // update AHRS system void Rover::ahrs_update() { - update_soft_armed(); + arming.update_soft_armed(); #if HIL_MODE != HIL_MODE_DISABLED // update hil before AHRS update diff --git a/APMrover2/AP_Arming.h b/APMrover2/AP_Arming.h index e6d9162f1e..b7c48d817d 100644 --- a/APMrover2/AP_Arming.h +++ b/APMrover2/AP_Arming.h @@ -20,9 +20,15 @@ public: bool pre_arm_rc_checks(const bool display_failure); bool gps_checks(bool display_failure) override; + bool disarm() override; + bool arm(AP_Arming::Method method, bool do_arming_checks=true) override; + + void update_soft_armed(); + protected: bool proximity_check(bool report); private: + void change_arm_state(void); }; diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index eb92f69432..169133fba3 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -614,23 +614,6 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l } return MAV_RESULT_FAILED; - case MAV_CMD_COMPONENT_ARM_DISARM: - if (is_equal(packet.param1, 1.0f)) { - // run pre_arm_checks and arm_checks and display failures - if (rover.arm_motors(AP_Arming::Method::MAVLINK)) { - return MAV_RESULT_ACCEPTED; - } else { - return MAV_RESULT_FAILED; - } - } else if (is_zero(packet.param1)) { - if (rover.disarm_motors()) { - return MAV_RESULT_ACCEPTED; - } else { - return MAV_RESULT_FAILED; - } - } - return MAV_RESULT_UNSUPPORTED; - case MAV_CMD_DO_CHANGE_SPEED: // param1 : unused // param2 : new speed in m/s diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 050ec0ea4d..e7f2039bf5 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -4,24 +4,6 @@ #if LOGGING_ENABLED == ENABLED -struct PACKED log_Arm_Disarm { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t arm_state; - uint16_t arm_checks; -}; - -void Rover::Log_Write_Arm_Disarm() -{ - struct log_Arm_Disarm pkt = { - LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG), - time_us : AP_HAL::micros64(), - arm_state : arming.is_armed(), - arm_checks : arming.get_enabled_checks() - }; - logger.WriteBlock(&pkt, sizeof(pkt)); -} - // Write an attitude packet void Rover::Log_Write_Attitude() { @@ -310,8 +292,6 @@ const LogStructure Rover::log_structure[] = { "NTUN", "QfHHHf", "TimeUS,WpDist,WpBrg,DesYaw,Yaw,XTrack", "smdddm", "F0BBB0" }, { LOG_RANGEFINDER_MSG, sizeof(log_Rangefinder), "RGFD", "QfHHHbHCb", "TimeUS,LatAcc,R1Dist,R2Dist,DCnt,TAng,TTim,Spd,Thr", "somm-hsm-", "F0BB-0CB-" }, - { LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm), - "ARM", "QBH", "TimeUS,ArmState,ArmChecks", "s--", "F--" }, { LOG_STEERING_MSG, sizeof(log_Steering), "STER", "Qhfffff", "TimeUS,SteerIn,SteerOut,DesLatAcc,LatAcc,DesTurnRate,TurnRate", "s--ookk", "F--0000" }, { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget), @@ -326,7 +306,6 @@ void Rover::log_init(void) #else // LOGGING_ENABLED // dummy functions -void Rover::Log_Write_Arm_Disarm() {} void Rover::Log_Write_Attitude() {} void Rover::Log_Write_Depth() {} void Rover::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} diff --git a/APMrover2/RC_Channel.cpp b/APMrover2/RC_Channel.cpp index 832ee9828f..ca92e7a281 100644 --- a/APMrover2/RC_Channel.cpp +++ b/APMrover2/RC_Channel.cpp @@ -134,15 +134,6 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi } break; - // arm or disarm the motors - case AUX_FUNC::ARMDISARM: - if (ch_flag == HIGH) { - rover.arm_motors(AP_Arming::Method::RUDDER); - } else if (ch_flag == LOW) { - rover.disarm_motors(); - } - break; - // set mode to Manual case AUX_FUNC::MANUAL: do_aux_function_change_mode(rover.mode_manual, ch_flag); diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 2ea85c4079..0b6cba744b 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -406,7 +406,6 @@ private: void send_wheel_encoder_distance(mavlink_channel_t chan); // Log.cpp - void Log_Write_Arm_Disarm(); void Log_Write_Attitude(); void Log_Write_Depth(); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); @@ -462,9 +461,6 @@ private: void notify_mode(const Mode *new_mode); uint8_t check_digital_pin(uint8_t pin); bool should_log(uint32_t mask); - void change_arm_state(void); - bool arm_motors(AP_Arming::Method method); - bool disarm_motors(void); bool is_boat() const; enum Failsafe_Action { @@ -492,7 +488,6 @@ private: public: void mavlink_delay_cb(); void failsafe_check(); - void update_soft_armed(); // Motor test void motor_test_output(); bool mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc, uint8_t motor_seq, uint8_t throttle_type, int16_t throttle_value); diff --git a/APMrover2/afs_rover.cpp b/APMrover2/afs_rover.cpp index 6c588e013a..4e4d1cf524 100644 --- a/APMrover2/afs_rover.cpp +++ b/APMrover2/afs_rover.cpp @@ -18,7 +18,7 @@ AP_AdvancedFailsafe_Rover::AP_AdvancedFailsafe_Rover(AP_Mission &_mission, const void AP_AdvancedFailsafe_Rover::terminate_vehicle(void) { // disarm as well - rover.disarm_motors(); + AP::arming().disarm(); // Set to HOLD mode rover.set_mode(rover.mode_hold, MODE_REASON_CRASH_FAILSAFE); diff --git a/APMrover2/crash_check.cpp b/APMrover2/crash_check.cpp index e4d378efe3..37d4e10c2a 100644 --- a/APMrover2/crash_check.cpp +++ b/APMrover2/crash_check.cpp @@ -51,14 +51,14 @@ void Rover::crash_check() if (is_balancebot()) { // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Disarming"); - disarm_motors(); + arming.disarm(); } else { // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD"); // change mode to hold and disarm set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE); if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) { - disarm_motors(); + arming.disarm(); } } } diff --git a/APMrover2/defines.h b/APMrover2/defines.h index a73207aec8..8d144d22dc 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -26,7 +26,6 @@ #define LOG_NTUN_MSG 0x02 #define LOG_STARTUP_MSG 0x06 #define LOG_RANGEFINDER_MSG 0x07 -#define LOG_ARM_DISARM_MSG 0x08 #define LOG_STEERING_MSG 0x0D #define LOG_GUIDEDTARGET_MSG 0x0E @@ -48,7 +47,7 @@ #define MASK_LOG_CAMERA (1<<12) #define MASK_LOG_STEERING (1<<13) #define MASK_LOG_RC (1<<14) -#define MASK_LOG_ARM_DISARM (1<<15) +// #define MASK_LOG_ARM_DISARM (1<<15) #define MASK_LOG_IMU_RAW (1UL<<19) // for mavlink SET_POSITION_TARGET messages diff --git a/APMrover2/failsafe.cpp b/APMrover2/failsafe.cpp index d367f4dae6..2bf237d935 100644 --- a/APMrover2/failsafe.cpp +++ b/APMrover2/failsafe.cpp @@ -35,7 +35,7 @@ void Rover::failsafe_check() // To-Do: log error if (arming.is_armed()) { // disarm motors - disarm_motors(); + arming.disarm(); } } } @@ -135,7 +135,7 @@ void Rover::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 - disarm_motors(); + arming.disarm(); #endif // ADVANCED_FAILSAFE == ENABLED break; } diff --git a/APMrover2/motor_test.cpp b/APMrover2/motor_test.cpp index 8dc69b0ce4..02f962286b 100644 --- a/APMrover2/motor_test.cpp +++ b/APMrover2/motor_test.cpp @@ -126,7 +126,7 @@ MAV_RESULT Rover::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor // arm motors if (!arming.is_armed()) { - arm_motors(AP_Arming::Method::MOTORTEST); + arming.arm(AP_Arming::Method::MOTORTEST); } // disable failsafes @@ -161,7 +161,7 @@ void Rover::motor_test_stop() } // disarm motors - disarm_motors(); + AP::arming().disarm(); // reset timeout motor_test_start_ms = 0; diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index dbbfc8671a..769dada988 100644 --- a/APMrover2/radio.cpp +++ b/APMrover2/radio.cpp @@ -72,7 +72,7 @@ void Rover::rudder_arm_disarm_check() } } else { // time to arm! - arm_motors(AP_Arming::Method::RUDDER); + arming.arm(AP_Arming::Method::RUDDER); rudder_arm_timer = 0; } } else { @@ -91,7 +91,7 @@ void Rover::rudder_arm_disarm_check() } } else { // time to disarm! - disarm_motors(); + arming.disarm(); rudder_arm_timer = 0; } } else { diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index fbccebd202..21bdd3fec5 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -317,7 +317,7 @@ bool Rover::should_log(uint32_t mask) /* update AHRS soft arm state and log as needed */ -void Rover::change_arm_state(void) +void AP_Arming_Rover::change_arm_state(void) { Log_Write_Arm_Disarm(); update_soft_armed(); @@ -326,15 +326,15 @@ void Rover::change_arm_state(void) /* arm motors */ -bool Rover::arm_motors(AP_Arming::Method method) +bool AP_Arming_Rover::arm(AP_Arming::Method method, const bool do_arming_checks) { - if (!arming.arm(method)) { + if (!AP_Arming::arm(method, do_arming_checks)) { AP_Notify::events.arming_failed = true; return false; } // Set the SmartRTL home location. If activated, SmartRTL will ultimately try to land at this point - g2.smart_rtl.set_home(true); + rover.g2.smart_rtl.set_home(true); // initialize simple mode heading rover.mode_simple.init_heading(); @@ -343,20 +343,21 @@ bool Rover::arm_motors(AP_Arming::Method method) rover.g2.windvane.record_home_heading(); change_arm_state(); + return true; } /* disarm motors */ -bool Rover::disarm_motors(void) +bool AP_Arming_Rover::disarm(void) { - if (!arming.disarm()) { + if (!AP_Arming::disarm()) { return false; } - if (control_mode != &mode_auto) { + if (rover.control_mode != &rover.mode_auto) { // reset the mission on disarm if we are not in auto - mode_auto.mission.reset(); + rover.mode_auto.mission.reset(); } // only log if disarming was successful