AP_Motors: always check armed and interlocked in motor test

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 @@ public:
     // 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 @@ protected:
     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 @@ public:
     // 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 @@ public:
     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 @@ public:
     // 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 @@ protected:
     // 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 @@ public:
     // 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 @@ protected:
     // 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 @@ public:
     // 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 @@ protected:
     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_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 @@ public:
     // 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 @@ protected:
 
     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 @@ public:
     // 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 @@ protected:
 
     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:

libraries/AP_Motors/AP_MotorsTri.h

@@ -32,11 +32,6 @@ public:
     // 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;
 
@@ -63,6 +58,11 @@ protected:
 
     const char* _get_frame_string() const override { return "TRI"; }
 
+    // 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;
+
     // parameters
 
     float           _pivot_angle;                       // Angle of yaw pivot

libraries/AP_Motors/AP_Motors_Class.cpp

@@ -256,6 +256,16 @@ void AP_Motors::set_frame_string(const char * str) {
 }
 #endif
 
+// 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_Motors::output_test_seq(uint8_t motor_seq, int16_t pwm)
+{
+    if (armed() && _interlock) {
+        _output_test_seq(motor_seq, pwm);
+    }
+}
+
 namespace AP {
     AP_Motors *motors()
     {

libraries/AP_Motors/AP_Motors_Class.h

@@ -218,7 +218,7 @@ public:
     // 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) = 0;
+    void                output_test_seq(uint8_t motor_seq, int16_t pwm);
 
     // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
     //  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
@@ -346,6 +346,11 @@ protected:
     // return string corresponding to frame_type
     virtual const char* get_type_string() const { return ""; }
 
+    // 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) = 0;
+
 #if AP_SCRIPTING_ENABLED
     // Custom frame string set from scripting
     char* custom_frame_string;

libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp

@@ -38,7 +38,7 @@ public:
     // have to have these functions as they are pure virtual
     void init(motor_frame_class frame_class, motor_frame_type frame_type) override {};
     void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) override {};
-    void output_test_seq(uint8_t motor_seq, int16_t pwm) override {};
+    void _output_test_seq(uint8_t motor_seq, int16_t pwm) override {};
     const char* _get_frame_string() const override { return "TEST"; }
     void output_armed_stabilizing() override {};
     void output_to_motors() override {};