Motor test, forward flight and turtle mode should all obey E-stop

ArduCopter/AP_Arming.cpp

@@ -642,12 +642,8 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
         return false;
     }
 
-    // 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)){
-        SRV_Channels::set_emergency_stop(false);
-        // if we are using motor Estop switch, it must not be in Estop position
-    } else if (SRV_Channels::get_emergency_stop()){
+    // if we are using motor Estop switch, it must not be in Estop position
+    if (SRV_Channels::get_emergency_stop()){
         check_failed(true, "Motor Emergency Stopped");
         return false;
     }

ArduCopter/mode_turtle.cpp

@@ -142,14 +142,17 @@ void ModeTurtle::run()
 // actually write values to the motors
 void ModeTurtle::output_to_motors()
 {
+    // check if motor are allowed to spin
+    const bool allow_output = motors->armed() && motors->get_interlock();
+
     for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
         if (!motors->is_motor_enabled(i)) {
             continue;
         }
 
         const Vector2f output{motors->get_roll_factor(i), motors->get_pitch_factor(i)};
         // if output aligns with input then use this motor
-        if ((motors_input - output).length() > 0.5) {
+        if (!allow_output || (motors_input - output).length() > 0.5) {
             motors->rc_write(i, motors->get_pwm_output_min());
             continue;
         }

ArduCopter/motor_test.cpp

@@ -121,6 +121,12 @@ bool Copter::mavlink_motor_control_check(const GCS_MAVLINK &gcs_chan, bool check
         return false;
     }
 
+    // check E-Stop is not active
+    if (SRV_Channels::get_emergency_stop()) {
+        gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: Motor Emergency Stopped", mode);
+        return false;
+    }
+
     // if we got this far the check was successful and the motor test can continue
     return true;
 }
@@ -135,8 +141,6 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t
     }
     // if test has not started try to start it
     if (!ap.motor_test) {
-        gcs().send_text(MAV_SEVERITY_INFO, "starting motor test");
-
         /* perform checks that it is ok to start test
            The RC calibrated check can be skipped if direct pwm is
            supplied
@@ -145,6 +149,7 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t
             return MAV_RESULT_FAILED;
         } else {
             // start test
+            gcs().send_text(MAV_SEVERITY_INFO, "starting motor test");
             ap.motor_test = true;
 
             EXPECT_DELAY_MS(3000);

ArduCopter/motors.cpp

@@ -161,19 +161,20 @@ void Copter::motors_output()
     // update output on any aux channels, for manual passthru
     SRV_Channels::output_ch_all();
 
-    // check if we are performing the motor test
+    // update motors interlock state
+    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);
+        AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_ENABLED);
+    } else if (motors->get_interlock() && !interlock) {
+        motors->set_interlock(false);
+        AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_DISABLED);
+    }
+
     if (ap.motor_test) {
+        // check if we are performing the motor test
         motor_test_output();
     } else {
-        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);
-            AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_ENABLED);
-        } else if (motors->get_interlock() && !interlock) {
-            motors->set_interlock(false);
-            AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_DISABLED);
-        }
-
         // send output signals to motors
         flightmode->output_to_motors();
     }

ArduPlane/quadplane.cpp

@@ -697,7 +697,6 @@ bool QuadPlane::setup(void)
     motors->init(frame_class, frame_type);
     motors->update_throttle_range();
     motors->set_update_rate(rc_speed);
-    motors->set_interlock(true);
     attitude_control->parameter_sanity_check();
 
     // setup the trim of any motors used by AP_Motors so I/O board
@@ -1664,6 +1663,9 @@ void QuadPlane::update(void)
         return;
     }
 
+    // keep motors interlock state upto date with E-stop
+    motors->set_interlock(!SRV_Channels::get_emergency_stop());
+
     if ((ahrs_view != NULL) && !is_equal(_last_ahrs_trim_pitch, ahrs_trim_pitch.get())) {
         _last_ahrs_trim_pitch = ahrs_trim_pitch.get();
         ahrs_view->set_pitch_trim(_last_ahrs_trim_pitch);

libraries/AP_Motors/AP_MotorsCoax.cpp

@@ -217,13 +217,8 @@ void AP_MotorsCoax::output_armed_stabilizing()
 // output_test_seq - spin a motor at the pwm value specified
 //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
 //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-void AP_MotorsCoax::output_test_seq(uint8_t motor_seq, int16_t pwm)
+void AP_MotorsCoax::_output_test_seq(uint8_t motor_seq, int16_t pwm)
 {
-    // exit immediately if not armed
-    if (!armed()) {
-        return;
-    }
-
     // output to motors and servos
     switch (motor_seq) {
         case 1:

libraries/AP_Motors/AP_MotorsCoax.h

@@ -37,11 +37,6 @@ class AP_MotorsCoax : public AP_MotorsMulticopter {
     // set update rate to motors - a value in hertz
     void                set_update_rate( uint16_t speed_hz ) override;
 
-    // output_test_seq - spin a motor at the pwm value specified
-    //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
-    //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-    virtual void        output_test_seq(uint8_t motor_seq, int16_t pwm) override;
-
     // output_to_motors - sends minimum values out to the motors
     virtual void        output_to_motors() override;
 
@@ -58,4 +53,9 @@ class AP_MotorsCoax : public AP_MotorsMulticopter {
     float               _thrust_yt_cw;
 
     const char* _get_frame_string() const override { return "COAX"; }
+
+    // output_test_seq - spin a motor at the pwm value specified
+    //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
+    //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
+    virtual void _output_test_seq(uint8_t motor_seq, int16_t pwm) override;
 };

libraries/AP_Motors/AP_MotorsHeli.h

@@ -60,11 +60,6 @@ class AP_MotorsHeli : public AP_Motors {
     // output_min - sets servos to neutral point with motors stopped
     void output_min() override;
 
-    // output_test_seq - spin a motor at the pwm value specified
-    //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
-    //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-    virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override = 0;
-
     //
     // heli specific methods
     //

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -275,13 +275,8 @@ bool AP_MotorsHeli_Dual::init_outputs()
 // output_test_seq - spin a motor at the pwm value specified
 //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
 //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-void AP_MotorsHeli_Dual::output_test_seq(uint8_t motor_seq, int16_t pwm)
+void AP_MotorsHeli_Dual::_output_test_seq(uint8_t motor_seq, int16_t pwm)
 {
-    // exit immediately if not armed
-    if (!armed()) {
-        return;
-    }
-
     // output to motors and servos
     switch (motor_seq) {
     case 1:

libraries/AP_Motors/AP_MotorsHeli_Dual.h

@@ -48,7 +48,7 @@ class AP_MotorsHeli_Dual : public AP_MotorsHeli {
     void set_update_rate( uint16_t speed_hz ) override;
 
     // output_test_seq - spin a motor at the pwm value specified
-    virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override;
+    virtual void _output_test_seq(uint8_t motor_seq, int16_t pwm) override;
 
     // output_to_motors - sends values out to the motors
     void output_to_motors() override;

libraries/AP_Motors/AP_MotorsHeli_Quad.cpp

@@ -75,13 +75,8 @@ bool AP_MotorsHeli_Quad::init_outputs()
 // output_test_seq - spin a motor at the pwm value specified
 //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
 //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-void AP_MotorsHeli_Quad::output_test_seq(uint8_t motor_seq, int16_t pwm)
+void AP_MotorsHeli_Quad::_output_test_seq(uint8_t motor_seq, int16_t pwm)
 {
-    // exit immediately if not armed
-    if (!armed()) {
-        return;
-    }
-
     // output to motors and servos
     switch (motor_seq) {
     case 1 ... AP_MOTORS_HELI_QUAD_NUM_MOTORS:

libraries/AP_Motors/AP_MotorsHeli_Quad.h

@@ -29,9 +29,6 @@ class AP_MotorsHeli_Quad : public AP_MotorsHeli {
     // set_update_rate - set update rate to motors
     void set_update_rate( uint16_t speed_hz ) override;
 
-    // output_test_seq - spin a motor at the pwm value specified
-    virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override;
-
     // output_to_motors - sends values out to the motors
     void output_to_motors() override;
 
@@ -88,6 +85,9 @@ class AP_MotorsHeli_Quad : public AP_MotorsHeli {
     // move_actuators - moves swash plate to attitude of parameters passed in
     void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out)  override;
 
+    // output_test_seq - spin a motor at the pwm value specified
+    virtual void _output_test_seq(uint8_t motor_seq, int16_t pwm) override;
+
     const char* _get_frame_string() const override { return "HELI_QUAD"; }
 
     // rate factors

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -223,13 +223,8 @@ bool AP_MotorsHeli_Single::init_outputs()
 // output_test_seq - spin a motor at the pwm value specified
 //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
 //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-void AP_MotorsHeli_Single::output_test_seq(uint8_t motor_seq, int16_t pwm)
+void AP_MotorsHeli_Single::_output_test_seq(uint8_t motor_seq, int16_t pwm)
 {
-    // exit immediately if not armed
-    if (!armed()) {
-        return;
-    }
-
     // output to motors and servos
     switch (motor_seq) {
         case 1:

libraries/AP_Motors/AP_MotorsHeli_Single.h

@@ -50,11 +50,6 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli {
     // set update rate to motors - a value in hertz
     void set_update_rate(uint16_t speed_hz) override;
 
-    // output_test_seq - spin a motor at the pwm value specified
-    //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
-    //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-    virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override;
-
     // output_to_motors - sends values out to the motors
     void output_to_motors() override;
 
@@ -120,6 +115,11 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli {
     // servo_test - move servos through full range of movement
     void servo_test() override;
 
+    // output_test_seq - spin a motor at the pwm value specified
+    //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
+    //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
+    virtual void _output_test_seq(uint8_t motor_seq, int16_t pwm) override;
+
     // external objects we depend upon
     AP_MotorsHeli_RSC   _tail_rotor;            // tail rotor
     AP_MotorsHeli_Swash _swashplate;            // swashplate

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -460,13 +460,8 @@ void AP_MotorsMatrix::check_for_failed_motor(float throttle_thrust_best_plus_adj
 // output_test_seq - spin a motor at the pwm value specified
 //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
 //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-void AP_MotorsMatrix::output_test_seq(uint8_t motor_seq, int16_t pwm)
+void AP_MotorsMatrix::_output_test_seq(uint8_t motor_seq, int16_t pwm)
 {
-    // exit immediately if not armed
-    if (!armed()) {
-        return;
-    }
-
     // loop through all the possible orders spinning any motors that match that description
     for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i] && _test_order[i] == motor_seq) {

libraries/AP_Motors/AP_MotorsMatrix.h

@@ -48,11 +48,6 @@ class AP_MotorsMatrix : public AP_MotorsMulticopter {
     // you must have setup_motors before calling this
     void                set_update_rate(uint16_t speed_hz) override;
 
-    // output_test_seq - spin a motor at the pwm value specified
-    //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
-    //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-    virtual void        output_test_seq(uint8_t motor_seq, int16_t pwm) override;
-
     // output_test_num - spin a motor connected to the specified output channel
     //  (should only be performed during testing)
     //  If a motor output channel is remapped, the mapped channel is used.
@@ -134,6 +129,11 @@ class AP_MotorsMatrix : public AP_MotorsMulticopter {
     const char*         _get_frame_string() const override { return _frame_class_string; }
     const char*         get_type_string() const override { return _frame_type_string; }
 
+    // output_test_seq - spin a motor at the pwm value specified
+    //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
+    //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
+    virtual void        _output_test_seq(uint8_t motor_seq, int16_t pwm) override;
+
     float               _roll_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to roll
     float               _pitch_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to pitch
     float               _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS];  // each motors contribution to yaw (normally 1 or -1)

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -758,7 +758,7 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask, float r
 
     for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i]) {
-            if ((mask & (1U << i)) && armed()) {
+            if ((mask & (1U << i)) && armed() && get_interlock()) {
                 /*
                  apply rudder mixing differential thrust
                  copter frame roll is plane frame yaw as this only

libraries/AP_Motors/AP_MotorsSingle.cpp

@@ -238,13 +238,8 @@ void AP_MotorsSingle::output_armed_stabilizing()
 // output_test_seq - spin a motor at the pwm value specified
 //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
 //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-void AP_MotorsSingle::output_test_seq(uint8_t motor_seq, int16_t pwm)
+void AP_MotorsSingle::_output_test_seq(uint8_t motor_seq, int16_t pwm)
 {
-    // exit immediately if not armed
-    if (!armed()) {
-        return;
-    }
-
     // output to motors and servos
     switch (motor_seq) {
         case 1:

libraries/AP_Motors/AP_MotorsSingle.h

@@ -37,11 +37,6 @@ class AP_MotorsSingle : public AP_MotorsMulticopter {
     // set update rate to motors - a value in hertz
     void                set_update_rate( uint16_t speed_hz ) override;
 
-    // output_test_seq - spin a motor at the pwm value specified
-    //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
-    //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-    virtual void        output_test_seq(uint8_t motor_seq, int16_t pwm) override;
-
     // output_to_motors - sends minimum values out to the motors
     virtual void        output_to_motors() override;
 
@@ -55,6 +50,11 @@ class AP_MotorsSingle : public AP_MotorsMulticopter {
 
     const char* _get_frame_string() const override { return "SINGLE"; }
 
+    // output_test_seq - spin a motor at the pwm value specified
+    //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
+    //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
+    virtual void        _output_test_seq(uint8_t motor_seq, int16_t pwm) override;
+
     int16_t             _throttle_radio_output;   // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
     float               _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range
     float               _thrust_out;

libraries/AP_Motors/AP_MotorsTailsitter.cpp

@@ -216,13 +216,8 @@ void AP_MotorsTailsitter::output_armed_stabilizing()
 // output_test_seq - spin a motor at the pwm value specified
 //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
 //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-void AP_MotorsTailsitter::output_test_seq(uint8_t motor_seq, int16_t pwm)
+void AP_MotorsTailsitter::_output_test_seq(uint8_t motor_seq, int16_t pwm)
 {
-    // exit immediately if not armed
-    if (!armed()) {
-        return;
-    }
-
     // output to motors and servos
     switch (motor_seq) {
         case 1:

libraries/AP_Motors/AP_MotorsTailsitter.h

@@ -23,9 +23,6 @@ class AP_MotorsTailsitter : public AP_MotorsMulticopter {
     // set update rate to motors - a value in hertz
     void set_update_rate( uint16_t speed_hz ) override;
 
-    // spin a motor at the pwm value specified
-    void output_test_seq(uint8_t motor_seq, int16_t pwm) override;
-
     // output_to_motors - sends output to named servos
     void output_to_motors() override;
 
@@ -42,6 +39,9 @@ class AP_MotorsTailsitter : public AP_MotorsMulticopter {
 
     const char* _get_frame_string() const override { return "TAILSITTER"; }
 
+    // spin a motor at the pwm value specified
+    void _output_test_seq(uint8_t motor_seq, int16_t pwm) override;
+
     // calculated outputs
     float _throttle; // 0..1
     float _tilt_left;  // -1..1

libraries/AP_Motors/AP_MotorsTri.cpp

@@ -280,13 +280,8 @@ void AP_MotorsTri::output_armed_stabilizing()
 // output_test_seq - spin a motor at the pwm value specified
 //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
 //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-void AP_MotorsTri::output_test_seq(uint8_t motor_seq, int16_t pwm)
+void AP_MotorsTri::_output_test_seq(uint8_t motor_seq, int16_t pwm)
 {
-    // exit immediately if not armed
-    if (!armed()) {
-        return;
-    }
-
     // output to motors and servos
     switch (motor_seq) {
         case 1: