mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
ArduCopter: log disarm method
This commit is contained in:
parent
98c999d0de
commit
ce5f23810b
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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:
|
||||||
|
@ -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
|
||||||
|
@ -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);
|
||||||
|
@ -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 {
|
||||||
|
|
||||||
|
@ -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();
|
||||||
|
@ -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
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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 {
|
||||||
|
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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;
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user