From ba3d1bc6c7570d8b278f5ad781a7637557b7b380 Mon Sep 17 00:00:00 2001 From: IamPete1 <33176108+IamPete1@users.noreply.github.com> Date: Fri, 25 Jan 2019 18:57:36 +0000 Subject: [PATCH] Copter: move E-stop state to SRV_Channel --- ArduCopter/AP_Arming.cpp | 4 ++-- ArduCopter/AP_State.cpp | 14 -------------- ArduCopter/Copter.h | 16 +++++++--------- ArduCopter/RC_Channel.cpp | 6 ------ ArduCopter/motors.cpp | 4 ++-- ArduCopter/radio.cpp | 2 +- 6 files changed, 12 insertions(+), 34 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index e95ee0d847..2109a63efc 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -565,9 +565,9 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, AP_Arming::ArmingMethod // if we are not using Emergency Stop switch option, force Estop false to ensure motors // can run normally if (!rc().find_channel_for_option(RC_Channel::aux_func::MOTOR_ESTOP)){ - copter.set_motor_emergency_stop(false); + SRV_Channels::set_emergency_stop(false); // if we are using motor Estop switch, it must not be in Estop position - } else if (rc().find_channel_for_option(RC_Channel::aux_func::MOTOR_ESTOP) && copter.ap.motor_emergency_stop){ + } else if (rc().find_channel_for_option(RC_Channel::aux_func::MOTOR_ESTOP) && SRV_Channels::get_emergency_stop()){ gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped"); return false; } diff --git a/ArduCopter/AP_State.cpp b/ArduCopter/AP_State.cpp index 0edb3069ed..6ad263e565 100644 --- a/ArduCopter/AP_State.cpp +++ b/ArduCopter/AP_State.cpp @@ -89,17 +89,3 @@ void Copter::update_using_interlock() ap.using_interlock = rc().find_channel_for_option(RC_Channel::aux_func::MOTOR_INTERLOCK) != nullptr; #endif } - -void Copter::set_motor_emergency_stop(bool b) -{ - if(ap.motor_emergency_stop != b) { - ap.motor_emergency_stop = b; - } - - // Log new status - if (ap.motor_emergency_stop){ - Log_Write_Event(DATA_MOTORS_EMERGENCY_STOPPED); - } else { - Log_Write_Event(DATA_MOTORS_EMERGENCY_STOP_CLEARED); - } -} diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index f0379f1caf..db5f5800a7 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -313,14 +313,13 @@ private: uint8_t system_time_set_unused : 1; // 16 // true if the system time has been set from the GPS uint8_t gps_glitching : 1; // 17 // true if GPS glitching is affecting navigation accuracy uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use - uint8_t motor_emergency_stop : 1; // 21 // motor estop switch, shuts off motors when enabled - uint8_t land_repo_active : 1; // 22 // true if the pilot is overriding the landing position - uint8_t motor_interlock_switch : 1; // 23 // true if pilot is requesting motor interlock enable - uint8_t in_arming_delay : 1; // 24 // true while we are armed but waiting to spin motors - uint8_t initialised_params : 1; // 25 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done - uint8_t compass_init_location : 1; // 26 // true when the compass's initial location has been set - uint8_t unused2 : 1; // 27 // aux switch rc_override is allowed - uint8_t armed_with_switch : 1; // 28 // we armed using a arming switch + uint8_t land_repo_active : 1; // 21 // true if the pilot is overriding the landing position + uint8_t motor_interlock_switch : 1; // 22 // true if pilot is requesting motor interlock enable + uint8_t in_arming_delay : 1; // 23 // true while we are armed but waiting to spin motors + uint8_t initialised_params : 1; // 24 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done + uint8_t compass_init_location : 1; // 25 // true when the compass's initial location has been set + uint8_t unused2 : 1; // 26 // aux switch rc_override is allowed + uint8_t armed_with_switch : 1; // 27 // we armed using a arming switch }; uint32_t value; } ap_t; @@ -613,7 +612,6 @@ private: void set_failsafe_radio(bool b); void set_failsafe_gcs(bool b); void update_using_interlock(); - void set_motor_emergency_stop(bool b); // ArduCopter.cpp void fast_loop(); diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 980c1c3e52..0038c7e5c4 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -74,7 +74,6 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const aux_ case MISSION_RESET: case ATTCON_FEEDFWD: case ATTCON_ACCEL_LIM: - case MOTOR_ESTOP: case MOTOR_INTERLOCK: case AVOID_ADSB: case PRECISION_LOITER: @@ -354,11 +353,6 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw #endif break; - case MOTOR_ESTOP: - // Turn on Emergency Stop logic when channel is high - copter.set_motor_emergency_stop(ch_flag == HIGH); - break; - case MOTOR_INTERLOCK: // Turn on when above LOW, because channel will also be used for speed // control signal in tradheli diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index fdc9fd7193..3db35a4d90 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -102,7 +102,7 @@ void Copter::auto_disarm_check() #endif // always allow auto disarm if using interlock switch or motors are Emergency Stopped - if ((ap.using_interlock && !motors->get_interlock()) || ap.motor_emergency_stop) { + if ((ap.using_interlock && !motors->get_interlock()) || SRV_Channels::get_emergency_stop()) { #if FRAME_CONFIG != HELI_FRAME // use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less // obvious the copter is armed as the motors will not be spinning @@ -330,7 +330,7 @@ void Copter::motors_output() if (ap.motor_test) { motor_test_output(); } else { - bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !ap.motor_emergency_stop; + bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !SRV_Channels::get_emergency_stop(); if (!motors->get_interlock() && interlock) { motors->set_interlock(true); Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED); diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 86cd003d99..19b656c40b 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -191,7 +191,7 @@ void Copter::set_throttle_zero_flag(int16_t throttle_control) // if not using throttle interlock and non-zero throttle and not E-stopped, // or using motor interlock and it's enabled, then motors are running, // and we are flying. Immediately set as non-zero - if ((!ap.using_interlock && (throttle_control > 0) && !ap.motor_emergency_stop) || + if ((!ap.using_interlock && (throttle_control > 0) && !SRV_Channels::get_emergency_stop()) || (ap.using_interlock && motors->get_interlock()) || ap.armed_with_switch) { last_nonzero_throttle_ms = tnow_ms;