From c16046aadfcb321e906f91b01c2b17d5e297500c Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 11 Apr 2017 12:45:16 -0400 Subject: [PATCH] Sub: Remove unused motor emergency stop and interlock --- ArduSub/AP_State.cpp | 14 -------------- ArduSub/Sub.h | 3 --- ArduSub/control_acro.cpp | 2 +- ArduSub/control_althold.cpp | 2 +- ArduSub/control_auto.cpp | 14 +++++++------- ArduSub/control_circle.cpp | 4 ++-- ArduSub/control_guided.cpp | 8 ++++---- ArduSub/control_manual.cpp | 2 +- ArduSub/control_poshold.cpp | 4 ++-- ArduSub/control_stabilize.cpp | 2 +- ArduSub/control_surface.cpp | 2 +- ArduSub/defines.h | 4 ---- ArduSub/motors.cpp | 11 ++--------- 13 files changed, 22 insertions(+), 50 deletions(-) diff --git a/ArduSub/AP_State.cpp b/ArduSub/AP_State.cpp index 6185f5a04b..e6fe3c8581 100644 --- a/ArduSub/AP_State.cpp +++ b/ArduSub/AP_State.cpp @@ -36,17 +36,3 @@ void Sub::set_auto_armed(bool b) Log_Write_Event(DATA_AUTO_ARMED); } } - -void Sub::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/ArduSub/Sub.h b/ArduSub/Sub.h index 50facb8145..a46a3f7259 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -239,8 +239,6 @@ private: uint8_t system_time_set : 1; // true if the system time has been set from the GPS uint8_t gps_base_pos_set : 1; // true when the gps base position has been set (used for RTK gps only) enum HomeState home_state : 2; // home status (unset, set, locked) - uint8_t using_interlock : 1; // aux switch motor interlock function is in use - uint8_t motor_emergency_stop: 1; // motor estop switch, shuts off motors when enabled uint8_t at_bottom : 1; // true if we are at the bottom uint8_t at_surface : 1; // true if we are at the surface uint8_t depth_sensor_present: 1; // true if we have an external baro connected @@ -470,7 +468,6 @@ private: void set_home_state(enum HomeState new_home_state); bool home_is_set(); void set_auto_armed(bool b); - void set_motor_emergency_stop(bool b); float get_smoothing_gain(); void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max); float get_pilot_desired_yaw_rate(int16_t stick_angle); diff --git a/ArduSub/control_acro.cpp b/ArduSub/control_acro.cpp index a8e8305b54..fa9d3600b1 100644 --- a/ArduSub/control_acro.cpp +++ b/ArduSub/control_acro.cpp @@ -21,7 +21,7 @@ void Sub::acro_run() float target_roll, target_pitch, target_yaw; // if not armed set throttle to zero and exit immediately - if (!motors.armed() || !motors.get_interlock()) { + if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 28a556658b..110adc4556 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -39,7 +39,7 @@ void Sub::althold_run() pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z); - if (!motors.armed() || !motors.get_interlock()) { + if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index 39aa9a40f5..254f199579 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -111,8 +111,8 @@ void Sub::auto_wp_start(const Location_Class& dest_loc) // called by auto_run at 100hz or more void Sub::auto_wp_run() { - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !motors.get_interlock()) { + // if not auto armed set throttle to zero and exit immediately + if (!motors.armed()) { // To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off // (of course it would be better if people just used take-off) // call attitude controller @@ -198,8 +198,8 @@ void Sub::auto_spline_start(const Location_Class& destination, bool stopped_at_s // called by auto_run at 100hz or more void Sub::auto_spline_run() { - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { + // if not auto armed set throttle to zero and exit immediately + if (!motors.armed() || !ap.auto_armed) { // To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off // (of course it would be better if people just used take-off) // Sub vehicles do not stabilize roll/pitch/yaw when disarmed @@ -376,8 +376,8 @@ bool Sub::auto_loiter_start() // called by auto_run at 100hz or more void Sub::auto_loiter_run() { - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !motors.get_interlock()) { + // if not auto armed set throttle to zero and exit immediately + if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); @@ -661,7 +661,7 @@ void Sub::auto_terrain_recover_run() static uint32_t rangefinder_recovery_ms = 0; // if not armed set throttle to zero and exit immediately - if (!motors.armed() || !motors.get_interlock()) { + if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index 48b8f4ca9d..e8e7b72ff3 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -39,8 +39,8 @@ void Sub::circle_run() pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z); - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { + // if not auto armed set throttle to zero and exit immediately + if (!motors.armed() || !ap.auto_armed) { // To-Do: add some initialisation of position controllers motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 37c52a62ba..98573f5471 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -278,7 +278,7 @@ void Sub::guided_run() void Sub::guided_pos_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { + if (!motors.armed() || !ap.auto_armed) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); @@ -327,7 +327,7 @@ void Sub::guided_pos_control_run() void Sub::guided_vel_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { + if (!motors.armed() || !ap.auto_armed) { // initialise velocity controller pos_control.init_vel_controller_xyz(); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); @@ -381,7 +381,7 @@ void Sub::guided_vel_control_run() void Sub::guided_posvel_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { + if (!motors.armed() || !ap.auto_armed) { // set target position and velocity to current position and velocity pos_control.set_pos_target(inertial_nav.get_position()); pos_control.set_desired_velocity(Vector3f(0,0,0)); @@ -457,7 +457,7 @@ void Sub::guided_posvel_control_run() void Sub::guided_angle_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { + if (!motors.armed() || !ap.auto_armed) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0.0f,true,g.throttle_filt); diff --git a/ArduSub/control_manual.cpp b/ArduSub/control_manual.cpp index a467ad9f4f..8edbd0bde0 100644 --- a/ArduSub/control_manual.cpp +++ b/ArduSub/control_manual.cpp @@ -14,7 +14,7 @@ bool Sub::manual_init(bool ignore_checks) void Sub::manual_run() { // if not armed set throttle to zero and exit immediately - if (!motors.armed() || !motors.get_interlock()) { + if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp index c774aa506e..0e57c79ff4 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -41,8 +41,8 @@ void Sub::poshold_run() // float vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw(); // float vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw(); - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { + // if not auto armed set throttle to zero and exit immediately + if (!motors.armed() || !ap.auto_armed) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); wp_nav.init_loiter_target(); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index b98f38251a..727ac7cc65 100644 --- a/ArduSub/control_stabilize.cpp +++ b/ArduSub/control_stabilize.cpp @@ -19,7 +19,7 @@ void Sub::stabilize_run() float target_yaw_rate; // if not armed set throttle to zero and exit immediately - if (!motors.armed() || !motors.get_interlock()) { + if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); last_pilot_heading = ahrs.yaw_sensor; diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp index a03d6f7a83..3f818d2df6 100644 --- a/ArduSub/control_surface.cpp +++ b/ArduSub/control_surface.cpp @@ -28,7 +28,7 @@ void Sub::surface_run() float target_yaw_rate; // if not armed set throttle to zero and exit immediately - if (!motors.armed() || !motors.get_interlock()) { + if (!motors.armed()) { motors.output_min(); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 98117f000f..6714116c5d 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -162,10 +162,6 @@ enum RTLState { #define DATA_ACRO_TRAINER_LIMITED 45 #define DATA_GRIPPER_GRAB 46 #define DATA_GRIPPER_RELEASE 47 -#define DATA_MOTORS_EMERGENCY_STOPPED 54 -#define DATA_MOTORS_EMERGENCY_STOP_CLEARED 55 -#define DATA_MOTORS_INTERLOCK_DISABLED 56 -#define DATA_MOTORS_INTERLOCK_ENABLED 57 #define DATA_EKF_ALT_RESET 60 #define DATA_SURFACE_CANCELLED_BY_PILOT 61 #define DATA_EKF_YAW_RESET 62 diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 560563d1a9..1e6590784e 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -130,16 +130,9 @@ void Sub::motors_output() // check if we are performing the motor test if (ap.motor_test) { return; // Placeholder - } else { - if (!ap.using_interlock) { - // if not using interlock switch, set according to Emergency Stop status - // where Emergency Stop is forced false during arming if Emergency Stop switch - // is not used. Interlock enabled means motors run, so we must - // invert motor_emergency_stop status for motors to run. - motors.set_interlock(!ap.motor_emergency_stop); - } - motors.output(); } + motors.set_interlock(true); + motors.output(); } // translate wpnav roll/pitch outputs to lateral/forward