Sub: Remove unused motor emergency stop and interlock

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);
-    }
-}

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);

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;

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);

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 not auto armed set throttle to zero and exit immediately
-    if (!motors.armed() || !motors.get_interlock()) {
+    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 not auto armed set throttle to zero and exit immediately
-    if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
+    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 not auto 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);
         // 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;

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 not auto armed set throttle to zero and exit immediately
-    if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
+    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

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);

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;

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 not auto armed 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);
         wp_nav.init_loiter_target();
         attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);

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;

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);

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

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