ArduCopter: log disarm method

This commit is contained in:
Peter Barker 2020-02-22 00:09:57 +11:00 committed by Andrew Tridgell
parent 98c999d0de
commit ce5f23810b
14 changed files with 24 additions and 24 deletions

View File

@ -827,14 +827,14 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_
} }
// arming.disarm - disarm motors // arming.disarm - disarm motors
bool AP_Arming_Copter::disarm() bool AP_Arming_Copter::disarm(const AP_Arming::Method method)
{ {
// return immediately if we are already disarmed // return immediately if we are already disarmed
if (!copter.motors->armed()) { if (!copter.motors->armed()) {
return true; return true;
} }
if (!AP_Arming::disarm()) { if (!AP_Arming::disarm(method)) {
return false; return false;
} }

View File

@ -23,7 +23,7 @@ public:
bool rc_calibration_checks(bool display_failure) override; bool rc_calibration_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; bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
protected: protected:

View File

@ -836,7 +836,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
if (copter.motors->armed()) { if (copter.motors->armed()) {
if (copter.ap.land_complete) { if (copter.ap.land_complete) {
// if landed, disarm motors // if landed, disarm motors
copter.arming.disarm(); copter.arming.disarm(AP_Arming::Method::SOLOPAUSEWHENLANDED);
} else { } else {
// assume that shots modes are all done in guided. // assume that shots modes are all done in guided.
// NOTE: this may need to change if we add a non-guided shot mode // NOTE: this may need to change if we add a non-guided shot mode
@ -1255,7 +1255,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_l
if (GCS_MAVLINK::handle_flight_termination(packet) != MAV_RESULT_ACCEPTED) { if (GCS_MAVLINK::handle_flight_termination(packet) != MAV_RESULT_ACCEPTED) {
#endif #endif
if (packet.param1 > 0.5f) { if (packet.param1 > 0.5f) {
copter.arming.disarm(); copter.arming.disarm(AP_Arming::Method::TERMINATION);
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} }
#if ADVANCED_FAILSAFE == ENABLED #if ADVANCED_FAILSAFE == ENABLED

View File

@ -25,7 +25,7 @@ void AP_AdvancedFailsafe_Copter::terminate_vehicle(void)
copter.motors->output(); copter.motors->output();
// disarm as well // disarm as well
copter.arming.disarm(); copter.arming.disarm(AP_Arming::Method::AFS);
// and set all aux channels // and set all aux channels
SRV_Channels::set_output_limit(SRV_Channel::k_heli_rsc, SRV_Channel::Limit::TRIM); SRV_Channels::set_output_limit(SRV_Channel::k_heli_rsc, SRV_Channel::Limit::TRIM);

View File

@ -34,7 +34,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O
// if landed and we will take some kind of action, just disarm // if landed and we will take some kind of action, just disarm
if ((actual_action > MAV_COLLISION_ACTION_REPORT) && copter.should_disarm_on_failsafe()) { if ((actual_action > MAV_COLLISION_ACTION_REPORT) && copter.should_disarm_on_failsafe()) {
copter.arming.disarm(); copter.arming.disarm(AP_Arming::Method::ADSBCOLLISIONACTION);
actual_action = MAV_COLLISION_ACTION_NONE; actual_action = MAV_COLLISION_ACTION_NONE;
} else { } else {

View File

@ -67,7 +67,7 @@ void Copter::crash_check()
// send message to gcs // send message to gcs
gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming"); gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming");
// disarm motors // disarm motors
copter.arming.disarm(); copter.arming.disarm(AP_Arming::Method::CRASH);
} }
} }
@ -234,7 +234,7 @@ void Copter::parachute_check()
void Copter::parachute_release() void Copter::parachute_release()
{ {
// disarm motors // disarm motors
arming.disarm(); arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE);
// release parachute // release parachute
parachute.release(); parachute.release();

View File

@ -41,7 +41,7 @@ void Copter::failsafe_radio_on_event()
if (should_disarm_on_failsafe()) { if (should_disarm_on_failsafe()) {
// should immediately disarm when we're on the ground // should immediately disarm when we're on the ground
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Disarming"); gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Disarming");
arming.disarm(); arming.disarm(AP_Arming::Method::RADIOFAILSAFE);
desired_action = Failsafe_Action_None; desired_action = Failsafe_Action_None;
} else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) { } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) {
@ -91,7 +91,7 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
// Conditions to deviate from BATT_FS_XXX_ACT parameter setting // Conditions to deviate from BATT_FS_XXX_ACT parameter setting
if (should_disarm_on_failsafe()) { if (should_disarm_on_failsafe()) {
// should immediately disarm when we're on the ground // should immediately disarm when we're on the ground
arming.disarm(); arming.disarm(AP_Arming::Method::BATTERYFAILSAFE);
desired_action = Failsafe_Action_None; desired_action = Failsafe_Action_None;
gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Disarming"); gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Disarming");
@ -175,7 +175,7 @@ void Copter::failsafe_gcs_on_event(void)
} else if (should_disarm_on_failsafe()) { } else if (should_disarm_on_failsafe()) {
// should immediately disarm when we're on the ground // should immediately disarm when we're on the ground
arming.disarm(); arming.disarm(AP_Arming::Method::GCSFAILSAFE);
desired_action = Failsafe_Action_None; desired_action = Failsafe_Action_None;
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Disarming"); gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Disarming");
@ -259,7 +259,7 @@ void Copter::failsafe_terrain_on_event()
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED); AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED);
if (should_disarm_on_failsafe()) { if (should_disarm_on_failsafe()) {
arming.disarm(); arming.disarm(AP_Arming::Method::TERRAINFAILSAFE);
#if MODE_RTL_ENABLED == ENABLED #if MODE_RTL_ENABLED == ENABLED
} else if (control_mode == Mode::Number::RTL) { } else if (control_mode == Mode::Number::RTL) {
mode_rtl.restart_without_terrain(); mode_rtl.restart_without_terrain();
@ -375,7 +375,7 @@ void Copter::do_failsafe_action(Failsafe_Action action, ModeReason reason){
snprintf(battery_type_str, 17, "%s battery", type_str); snprintf(battery_type_str, 17, "%s battery", type_str);
g2.afs.gcs_terminate(true, battery_type_str); g2.afs.gcs_terminate(true, battery_type_str);
#else #else
arming.disarm(); arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE);
#endif #endif
} }
} }

View File

@ -30,7 +30,7 @@ void Copter::fence_check()
// disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle // disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
// don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
if (ap.land_complete || (flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){ if (ap.land_complete || (flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){
arming.disarm(); arming.disarm(AP_Arming::Method::FENCEBREACH);
} else { } else {

View File

@ -120,7 +120,7 @@ void Copter::set_land_complete(bool b)
const bool mode_disarms_on_land = flightmode->allows_arming(false) && !flightmode->has_manual_throttle(); const bool mode_disarms_on_land = flightmode->allows_arming(false) && !flightmode->has_manual_throttle();
if (ap.land_complete && motors->armed() && disarm_on_land_configured && mode_disarms_on_land) { if (ap.land_complete && motors->armed() && disarm_on_land_configured && mode_disarms_on_land) {
arming.disarm(); arming.disarm(AP_Arming::Method::LANDED);
} }
} }

View File

@ -528,7 +528,7 @@ void ModeAuto::exit_mission()
} }
} else { } else {
// if we've landed it's safe to disarm // if we've landed it's safe to disarm
copter.arming.disarm(); copter.arming.disarm(AP_Arming::Method::MISSIONEXIT);
} }
} }

View File

@ -58,7 +58,7 @@ void ModeLand::gps_run()
{ {
// disarm when the landing detector says we've landed // disarm when the landing detector says we've landed
if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
copter.arming.disarm(); copter.arming.disarm(AP_Arming::Method::LANDED);
} }
// Land State Machine Determination // Land State Machine Determination
@ -113,7 +113,7 @@ void ModeLand::nogps_run()
// disarm when the landing detector says we've landed // disarm when the landing detector says we've landed
if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
copter.arming.disarm(); copter.arming.disarm(AP_Arming::Method::LANDED);
} }
// Land State Machine Determination // Land State Machine Determination

View File

@ -386,7 +386,7 @@ void ModeRTL::land_run(bool disarm_on_land)
// disarm when the landing detector says we've landed // disarm when the landing detector says we've landed
if (disarm_on_land && copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { if (disarm_on_land && copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
copter.arming.disarm(); copter.arming.disarm(AP_Arming::Method::LANDED);
} }
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately

View File

@ -71,7 +71,7 @@ void Copter::arm_motors_check()
// disarm the motors // disarm the motors
if (arming_counter == DISARM_DELAY && motors->armed()) { if (arming_counter == DISARM_DELAY && motors->armed()) {
arming.disarm(); arming.disarm(AP_Arming::Method::RUDDER);
} }
// Yaw is centered so reset arming counter // Yaw is centered so reset arming counter
@ -124,7 +124,7 @@ void Copter::auto_disarm_check()
// disarm once timer expires // disarm once timer expires
if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) { if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) {
arming.disarm(); arming.disarm(AP_Arming::Method::DISARMDELAY);
auto_disarm_begin = tnow_ms; auto_disarm_begin = tnow_ms;
} }
} }

View File

@ -410,7 +410,7 @@ void ToyMode::update()
const uint8_t disarm_limit = copter.flightmode->has_manual_throttle()?TOY_LAND_MANUAL_DISARM_COUNT:TOY_LAND_DISARM_COUNT; const uint8_t disarm_limit = copter.flightmode->has_manual_throttle()?TOY_LAND_MANUAL_DISARM_COUNT:TOY_LAND_DISARM_COUNT;
if (throttle_low_counter >= disarm_limit) { if (throttle_low_counter >= disarm_limit) {
gcs().send_text(MAV_SEVERITY_INFO, "Tmode: throttle disarm"); gcs().send_text(MAV_SEVERITY_INFO, "Tmode: throttle disarm");
copter.arming.disarm(); copter.arming.disarm(AP_Arming::Method::TLANDTHROTTLE);
} }
} else { } else {
throttle_low_counter = 0; throttle_low_counter = 0;
@ -569,7 +569,7 @@ void ToyMode::update()
case ACTION_DISARM: case ACTION_DISARM:
if (copter.motors->armed()) { if (copter.motors->armed()) {
gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: Force disarm"); gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: Force disarm");
copter.arming.disarm(); copter.arming.disarm(AP_Arming::Method::TLANDFORCE);
} }
break; break;