From 98c999d0de2df4f2fd009eb78ff00dc414530ef2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 22 Feb 2020 00:09:57 +1100 Subject: [PATCH] APMrover2: log disarm method --- APMrover2/AP_Arming.cpp | 4 ++-- APMrover2/AP_Arming.h | 2 +- APMrover2/afs_rover.cpp | 2 +- APMrover2/crash_check.cpp | 4 ++-- APMrover2/failsafe.cpp | 4 ++-- APMrover2/motor_test.cpp | 2 +- APMrover2/radio.cpp | 2 +- 7 files changed, 10 insertions(+), 10 deletions(-) diff --git a/APMrover2/AP_Arming.cpp b/APMrover2/AP_Arming.cpp index 56ce4fc25e..b3ddfbf1ea 100644 --- a/APMrover2/AP_Arming.cpp +++ b/APMrover2/AP_Arming.cpp @@ -135,9 +135,9 @@ bool AP_Arming_Rover::arm(AP_Arming::Method method, const bool do_arming_checks) /* disarm motors */ -bool AP_Arming_Rover::disarm(void) +bool AP_Arming_Rover::disarm(const AP_Arming::Method method) { - if (!AP_Arming::disarm()) { + if (!AP_Arming::disarm(method)) { return false; } if (rover.control_mode != &rover.mode_auto) { diff --git a/APMrover2/AP_Arming.h b/APMrover2/AP_Arming.h index fb7fda6be3..72d546d80c 100644 --- a/APMrover2/AP_Arming.h +++ b/APMrover2/AP_Arming.h @@ -21,7 +21,7 @@ public: bool rc_calibration_checks(const bool display_failure) override; bool gps_checks(bool display_failure) override; - bool disarm() override; + bool disarm(AP_Arming::Method method) override; bool arm(AP_Arming::Method method, bool do_arming_checks=true) override; void update_soft_armed(); diff --git a/APMrover2/afs_rover.cpp b/APMrover2/afs_rover.cpp index 3f2aad211c..e0349eefc0 100644 --- a/APMrover2/afs_rover.cpp +++ b/APMrover2/afs_rover.cpp @@ -12,7 +12,7 @@ void AP_AdvancedFailsafe_Rover::terminate_vehicle(void) { // disarm as well - AP::arming().disarm(); + AP::arming().disarm(AP_Arming::Method::AFS); // Set to HOLD mode rover.set_mode(rover.mode_hold, ModeReason::CRASH_FAILSAFE); diff --git a/APMrover2/crash_check.cpp b/APMrover2/crash_check.cpp index 9752668c7c..284c125a4b 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"); - arming.disarm(); + arming.disarm(AP_Arming::Method::CRASH); } 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, ModeReason::CRASH_FAILSAFE); if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) { - arming.disarm(); + arming.disarm(AP_Arming::Method::CRASH); } } } diff --git a/APMrover2/failsafe.cpp b/APMrover2/failsafe.cpp index 3df6e10848..9d2ed89f6d 100644 --- a/APMrover2/failsafe.cpp +++ b/APMrover2/failsafe.cpp @@ -37,7 +37,7 @@ void Rover::failsafe_check() // To-Do: log error if (arming.is_armed()) { // disarm motors - arming.disarm(); + arming.disarm(AP_Arming::Method::CPUFAILSAFE); } } } @@ -137,7 +137,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 - arming.disarm(); + arming.disarm(AP_Arming::Method::BATTERYFAILSAFE); #endif // ADVANCED_FAILSAFE == ENABLED break; } diff --git a/APMrover2/motor_test.cpp b/APMrover2/motor_test.cpp index 185ff3c8c1..81324b3be8 100644 --- a/APMrover2/motor_test.cpp +++ b/APMrover2/motor_test.cpp @@ -159,7 +159,7 @@ void Rover::motor_test_stop() } // disarm motors - AP::arming().disarm(); + AP::arming().disarm(AP_Arming::Method::MOTORTEST); // reset timeout motor_test_start_ms = 0; diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index 28b5edd8df..80f43b4766 100644 --- a/APMrover2/radio.cpp +++ b/APMrover2/radio.cpp @@ -98,7 +98,7 @@ void Rover::rudder_arm_disarm_check() } } else { // time to disarm! - arming.disarm(); + arming.disarm(AP_Arming::Method::RUDDER); rudder_arm_timer = 0; } } else {