AP_MotorsHeli: add spool logic support

Also moved heli init_target_on_arming flag in from vehicle code

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -229,6 +229,10 @@ void AP_MotorsHeli::init(motor_frame_class frame_class, motor_frame_type frame_t
 
     // record successful initialisation if what we setup was the desired frame_class
     _flags.initialised_ok = (frame_class == MOTOR_FRAME_HELI);
+
+    // set flag to true so targets are initialized once aircraft is armed for first time
+    _heliflags.init_targets_on_arming = true;
+
 }
 
 // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
@@ -258,6 +262,9 @@ void AP_MotorsHeli::output()
     // update throttle filter
     update_throttle_filter();
 
+    // run spool logic
+    output_logic();
+
     if (_flags.armed) {
         calculate_armed_scalars();
         if (!_flags.interlock) {
@@ -268,6 +275,9 @@ void AP_MotorsHeli::output()
     } else {
         output_disarmed();
     }
+    
+    output_to_motors();
+
 };
 
 // sends commands to the motors
@@ -279,8 +289,6 @@ void AP_MotorsHeli::output_armed_stabilizing()
     }
 
     move_actuators(_roll_in, _pitch_in, get_throttle(), _yaw_in);
-
-    update_motor_control(ROTOR_CONTROL_ACTIVE);
 }
 
 // output_armed_zero_throttle - sends commands to the motors
@@ -292,8 +300,6 @@ void AP_MotorsHeli::output_armed_zero_throttle()
     }
 
     move_actuators(_roll_in, _pitch_in, get_throttle(), _yaw_in);
-
-    update_motor_control(ROTOR_CONTROL_IDLE);
 }
 
 // output_disarmed - sends commands to the motors
@@ -351,8 +357,92 @@ void AP_MotorsHeli::output_disarmed()
 
     // helicopters always run stabilizing flight controls
     move_actuators(_roll_in, _pitch_in, get_throttle(), _yaw_in);
+}
 
-    update_motor_control(ROTOR_CONTROL_STOP);
+// run spool logic
+void AP_MotorsHeli::output_logic()
+{
+    // force desired and current spool mode if disarmed and armed with interlock enabled
+    if (_flags.armed) {
+        if (!_flags.interlock) {
+            _spool_desired = DESIRED_GROUND_IDLE;
+        } else {
+            _heliflags.init_targets_on_arming = false;
+        }
+    } else {
+        _heliflags.init_targets_on_arming = true;
+        _spool_desired = DESIRED_SHUT_DOWN;
+        _spool_mode = SHUT_DOWN;
+    }
+
+    switch (_spool_mode) {
+        case SHUT_DOWN:
+            // Motors should be stationary.
+            // Servos set to their trim values or in a test condition.
+
+            // make sure the motors are spooling in the correct direction
+            if (_spool_desired != DESIRED_SHUT_DOWN) {
+                _spool_mode = GROUND_IDLE;
+                break;
+            }
+
+            break;
+
+        case GROUND_IDLE: {
+            // Motors should be stationary or at ground idle.
+            // Servos should be moving to correct the current attitude.
+            if (_spool_desired == DESIRED_SHUT_DOWN){
+                _spool_mode = SHUT_DOWN;
+            } else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
+                _spool_mode = SPOOL_UP;
+            } else {    // _spool_desired == GROUND_IDLE
+
+            }
+
+            break;
+        }
+        case SPOOL_UP:
+            // Maximum throttle should move from minimum to maximum.
+            // Servos should exhibit normal flight behavior.
+
+            // make sure the motors are spooling in the correct direction
+            if (_spool_desired != DESIRED_THROTTLE_UNLIMITED ){
+                _spool_mode = SPOOL_DOWN;
+                break;
+            }
+
+            if (_heliflags.rotor_runup_complete){
+                _spool_mode = THROTTLE_UNLIMITED;
+            }
+            break;
+
+        case THROTTLE_UNLIMITED:
+            // Throttle should exhibit normal flight behavior.
+            // Servos should exhibit normal flight behavior.
+
+            // make sure the motors are spooling in the correct direction
+            if (_spool_desired != DESIRED_THROTTLE_UNLIMITED) {
+                _spool_mode = SPOOL_DOWN;
+                break;
+            }
+
+
+            break;
+
+        case SPOOL_DOWN:
+            // Maximum throttle should move from maximum to minimum.
+            // Servos should exhibit normal flight behavior.
+
+            // make sure the motors are spooling in the correct direction
+            if (_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
+                _spool_mode = SPOOL_UP;
+                break;
+            }
+            if (!rotor_speed_above_critical()){
+                _spool_mode = GROUND_IDLE;
+            }
+            break;
+    }
 }
 
 // parameter_check - check if helicopter specific parameters are sensible

libraries/AP_Motors/AP_MotorsHeli.h

@@ -129,6 +129,9 @@ public:
 
     float get_throttle_hover() const override { return 0.5f; }
 
+    // support passing init_targets_on_arming flag to greater code
+    bool init_targets_on_arming() const { return _heliflags.init_targets_on_arming; }
+
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -152,6 +155,12 @@ protected:
     // update_motor_controls - sends commands to motor controllers
     virtual void update_motor_control(RotorControlState state) = 0;
 
+    // run spool logic
+    void                output_logic();
+
+    // output_to_motors - sends commands to the motors
+    virtual void        output_to_motors() = 0;
+
     // reset_flight_controls - resets all controls and scalars to flight status
     void reset_flight_controls();
 
@@ -188,6 +197,7 @@ protected:
         uint8_t landing_collective      : 1;    // true if collective is setup for landing which has much higher minimum
         uint8_t rotor_runup_complete    : 1;    // true if the rotors have had enough time to wind up
         uint8_t inverted_flight         : 1;    // true for inverted flight
+        uint8_t init_targets_on_arming  : 1;    // 0 if targets were initialized, 1 if targets were not initialized after arming
     } _heliflags;
 
     // parameters

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -506,27 +506,51 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
     _rotor.set_collective(fabsf(collective_out));
 
     // swashplate servos
-    float servo_out[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
-    
-    servo_out[CH_1] = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out + _yawFactor[CH_1] * yaw_out)*0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
-    servo_out[CH_2] = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out + _yawFactor[CH_2] * yaw_out)*0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
-    servo_out[CH_3] = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out + _yawFactor[CH_3] * yaw_out)*0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
+    _servo_out[CH_1] = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out + _yawFactor[CH_1] * yaw_out)*0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
+    _servo_out[CH_2] = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out + _yawFactor[CH_2] * yaw_out)*0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
+    _servo_out[CH_3] = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out + _yawFactor[CH_3] * yaw_out)*0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
 
-    servo_out[CH_4] = (_rollFactor[CH_4] * roll_out + _pitchFactor[CH_4] * pitch_out + _yawFactor[CH_4] * yaw_out)*0.45f + _collectiveFactor[CH_4] * collective2_out_scaled;
-    servo_out[CH_5] = (_rollFactor[CH_5] * roll_out + _pitchFactor[CH_5] * pitch_out + _yawFactor[CH_5] * yaw_out)*0.45f + _collectiveFactor[CH_5] * collective2_out_scaled;
-    servo_out[CH_6] = (_rollFactor[CH_6] * roll_out + _pitchFactor[CH_6] * pitch_out + _yawFactor[CH_6] * yaw_out)*0.45f + _collectiveFactor[CH_6] * collective2_out_scaled;
+    _servo_out[CH_4] = (_rollFactor[CH_4] * roll_out + _pitchFactor[CH_4] * pitch_out + _yawFactor[CH_4] * yaw_out)*0.45f + _collectiveFactor[CH_4] * collective2_out_scaled;
+    _servo_out[CH_5] = (_rollFactor[CH_5] * roll_out + _pitchFactor[CH_5] * pitch_out + _yawFactor[CH_5] * yaw_out)*0.45f + _collectiveFactor[CH_5] * collective2_out_scaled;
+    _servo_out[CH_6] = (_rollFactor[CH_6] * roll_out + _pitchFactor[CH_6] * pitch_out + _yawFactor[CH_6] * yaw_out)*0.45f + _collectiveFactor[CH_6] * collective2_out_scaled;
 
     // rescale from -1..1, so we can use the pwm calc that includes trim
     for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
-        servo_out[i] = 2*servo_out[i] - 1;
+        _servo_out[i] = 2*_servo_out[i] - 1;
     }
+}
 
+void AP_MotorsHeli_Dual::output_to_motors()
+{
+    if (!_flags.initialised_ok) {
+        return;
+    }
     // actually move the servos.  PWM is sent based on nominal 1500 center.  servo output shifts center based on trim value.
     for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
-        rc_write_swash(i, servo_out[i]);
+        rc_write_swash(i, _servo_out[i]);
     }
-}
 
+    switch (_spool_mode) {
+        case SHUT_DOWN:
+            // sends minimum values out to the motors
+            update_motor_control(ROTOR_CONTROL_STOP);
+            break;
+        case GROUND_IDLE:
+            // sends idle output to motors when armed. rotor could be static or turning (autorotation)
+            update_motor_control(ROTOR_CONTROL_IDLE);
+            break;
+        case SPOOL_UP:
+        case THROTTLE_UNLIMITED:
+            // set motor output based on thrust requests
+            update_motor_control(ROTOR_CONTROL_ACTIVE);
+            break;
+        case SPOOL_DOWN:
+            // sends idle output to motors and wait for rotor to stop
+            update_motor_control(ROTOR_CONTROL_IDLE);
+            break;
+
+    }
+}
 
 // servo_test - move servos through full range of movement
 void AP_MotorsHeli_Dual::servo_test()

libraries/AP_Motors/AP_MotorsHeli_Dual.h

@@ -61,6 +61,9 @@ public:
     // 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;
+
     // set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
     void set_desired_rotor_speed(float desired_speed) override;
 
@@ -117,6 +120,7 @@ protected:
     float _collective_test = 0.0f;                  // over-ride for collective output, used by servo_test function
     float _roll_test = 0.0f;                        // over-ride for roll output, used by servo_test function
     float _pitch_test = 0.0f;                       // over-ride for pitch output, used by servo_test function
+    float _servo_out[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];    // output value sent to motor
 
     // parameters
     AP_Int16        _collective2_min;               // Lowest possible servo position for the rear swashplate

libraries/AP_Motors/AP_MotorsHeli_Quad.cpp

@@ -232,10 +232,8 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
     // reserve some collective for attitude control
     collective_out *= collective_range;
 
-    float out[AP_MOTORS_HELI_QUAD_NUM_MOTORS] {};
-
     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
-        out[i] =
+        _out[i] =
             _rollFactor[CH_1+i] * roll_out +
             _pitchFactor[CH_1+i] * pitch_out +
             _collectiveFactor[CH_1+i] * collective_out;
@@ -244,16 +242,16 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
     // see if we need to scale down yaw_out
     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
         float y = _yawFactor[CH_1+i] * yaw_out;
-        if (out[i] < 0) {
+        if (_out[i] < 0) {
             // the slope of the yaw effect changes at zero collective
             y = -y;
         }
-        if (out[i] * (out[i] + y) < 0) {
+        if (_out[i] * (_out[i] + y) < 0) {
             // applying this yaw demand would change the sign of the
             // collective, which means the yaw would not be applied
             // evenly. We scale down the overall yaw demand to prevent
             // it crossing over zero
-            float s = -(out[i] / y);
+            float s = -(_out[i] / y);
             yaw_out *= s;
         }
     }
@@ -261,19 +259,46 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
     // now apply the yaw correction
     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
         float y = _yawFactor[CH_1+i] * yaw_out;
-        if (out[i] < 0) {
+        if (_out[i] < 0) {
             // the slope of the yaw effect changes at zero collective
             y = -y;
         }
-        out[i] += y;
+        _out[i] += y;
+    }
+
+}
+
+void AP_MotorsHeli_Quad::output_to_motors()
+{
+    if (!_flags.initialised_ok) {
+        return;
     }
 
     // move the servos
     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
-        rc_write_angle(AP_MOTORS_MOT_1+i, out[i] * QUAD_SERVO_MAX_ANGLE);
+        rc_write_angle(AP_MOTORS_MOT_1+i, _out[i] * QUAD_SERVO_MAX_ANGLE);
     }
-}
 
+    switch (_spool_mode) {
+        case SHUT_DOWN:
+            // sends minimum values out to the motors
+            update_motor_control(ROTOR_CONTROL_STOP);
+            break;
+        case GROUND_IDLE:
+            // sends idle output to motors when armed. rotor could be static or turning (autorotation)
+            update_motor_control(ROTOR_CONTROL_IDLE);
+            break;
+        case SPOOL_UP:
+        case THROTTLE_UNLIMITED:
+            // set motor output based on thrust requests
+            update_motor_control(ROTOR_CONTROL_ACTIVE);
+            break;
+        case SPOOL_DOWN:
+            // sends idle output to motors and wait for rotor to stop
+            update_motor_control(ROTOR_CONTROL_IDLE);
+            break;
+    }
+}
 
 // servo_test - move servos through full range of movement
 void AP_MotorsHeli_Quad::servo_test()

libraries/AP_Motors/AP_MotorsHeli_Quad.h

@@ -36,6 +36,9 @@ public:
     // 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;
+
     // set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
     void set_desired_rotor_speed(float desired_speed) override;
 
@@ -91,6 +94,7 @@ protected:
     float _pitchFactor[AP_MOTORS_HELI_QUAD_NUM_MOTORS];
     float _collectiveFactor[AP_MOTORS_HELI_QUAD_NUM_MOTORS];
     float _yawFactor[AP_MOTORS_HELI_QUAD_NUM_MOTORS];
+    float _out[AP_MOTORS_HELI_QUAD_NUM_MOTORS];
 };
 
 

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -461,23 +461,18 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
     if (_collective_direction == AP_MOTORS_HELI_SINGLE_COLLECTIVE_DIRECTION_REVERSED){
         collective_out_scaled = 1 - collective_out_scaled;
     }
-    float servo1_out = ((_rollFactor[CH_1] * roll_out) + (_pitchFactor[CH_1] * pitch_out))*0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
-    float servo2_out = ((_rollFactor[CH_2] * roll_out) + (_pitchFactor[CH_2] * pitch_out))*0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
+    _servo1_out = ((_rollFactor[CH_1] * roll_out) + (_pitchFactor[CH_1] * pitch_out))*0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
+    _servo2_out = ((_rollFactor[CH_2] * roll_out) + (_pitchFactor[CH_2] * pitch_out))*0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
     if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H1) {
-        servo1_out += 0.5f;
-        servo2_out += 0.5f;
+        _servo1_out += 0.5f;
+        _servo2_out += 0.5f;
     }
-    float servo3_out = ((_rollFactor[CH_3] * roll_out) + (_pitchFactor[CH_3] * pitch_out))*0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
+    _servo3_out = ((_rollFactor[CH_3] * roll_out) + (_pitchFactor[CH_3] * pitch_out))*0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
 
     // rescale from -1..1, so we can use the pwm calc that includes trim
-    servo1_out = 2*servo1_out - 1;
-    servo2_out = 2*servo2_out - 1;
-    servo3_out = 2*servo3_out - 1;
-
-    // actually move the servos.  PWM is sent based on nominal 1500 center.  servo output shifts center based on trim value.
-    rc_write_swash(AP_MOTORS_MOT_1, servo1_out);
-    rc_write_swash(AP_MOTORS_MOT_2, servo2_out);
-    rc_write_swash(AP_MOTORS_MOT_3, servo3_out);
+    _servo1_out = 2*_servo1_out - 1;
+    _servo2_out = 2*_servo2_out - 1;
+    _servo3_out = 2*_servo3_out - 1;
 
     // update the yaw rate using the tail rotor/servo
     move_yaw(yaw_out + yaw_offset);
@@ -496,18 +491,21 @@ void AP_MotorsHeli_Single::move_yaw(float yaw_out)
         limit.yaw = true;
     }
 
-    if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
-        if (_main_rotor.get_desired_speed() > 0.0f && hal.util->get_soft_armed()) {
-            // constrain output so that motor never fully stops
-            yaw_out = constrain_float(yaw_out, -0.9f, 1.0f);
-            // output yaw servo to tail rsc
-            rc_write_angle(AP_MOTORS_MOT_4, yaw_out * YAW_SERVO_MAX_ANGLE);
-        } else {
-            // output zero speed to tail rsc
-            rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
-        }
-    } else {
-        rc_write_angle(AP_MOTORS_MOT_4, yaw_out * YAW_SERVO_MAX_ANGLE);
+    _servo4_out = yaw_out;
+}
+
+void AP_MotorsHeli_Single::output_to_motors()
+{
+    if (!_flags.initialised_ok) {
+        return;
+    }
+
+    // actually move the servos.  PWM is sent based on nominal 1500 center.  servo output shifts center based on trim value.
+    rc_write_swash(AP_MOTORS_MOT_1, _servo1_out);
+    rc_write_swash(AP_MOTORS_MOT_2, _servo2_out);
+    rc_write_swash(AP_MOTORS_MOT_3, _servo3_out);
+    if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
+        rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE);
     }
     if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
         // output gain to exernal gyro
@@ -517,6 +515,42 @@ void AP_MotorsHeli_Single::move_yaw(float yaw_out)
             rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_std);
         }
     }
+
+    switch (_spool_mode) {
+        case SHUT_DOWN:
+            // sends minimum values out to the motors
+            update_motor_control(ROTOR_CONTROL_STOP);
+            if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
+                rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
+            }
+            break;
+        case GROUND_IDLE:
+            // sends idle output to motors when armed. rotor could be static or turning (autorotation)
+            update_motor_control(ROTOR_CONTROL_IDLE);
+            if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
+                rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
+            }
+            break;
+        case SPOOL_UP:
+        case THROTTLE_UNLIMITED:
+            // set motor output based on thrust requests
+            update_motor_control(ROTOR_CONTROL_ACTIVE);
+            if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
+                // constrain output so that motor never fully stops
+                 _servo4_out = constrain_float(_servo4_out, -0.9f, 1.0f);
+                // output yaw servo to tail rsc
+                rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE);
+            }
+            break;
+        case SPOOL_DOWN:
+            // sends idle output to motors and wait for rotor to stop
+            update_motor_control(ROTOR_CONTROL_IDLE);
+            if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
+                rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
+            }
+            break;
+
+    }
 }
 
 // servo_test - move servos through full range of movement

libraries/AP_Motors/AP_MotorsHeli_Single.h

@@ -66,6 +66,9 @@ public:
     //  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;
+
     // set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1
     void set_desired_rotor_speed(float desired_speed) override;
 
@@ -136,6 +139,10 @@ protected:
     float _roll_test = 0.0f;                    // over-ride for roll output, used by servo_test function
     float _pitch_test = 0.0f;                   // over-ride for pitch output, used by servo_test function
     float _yaw_test = 0.0f;                     // over-ride for yaw output, used by servo_test function
+    float _servo1_out = 0.0f;                   // output value sent to motor
+    float _servo2_out = 0.0f;                   // output value sent to motor
+    float _servo3_out = 0.0f;                   // output value sent to motor
+    float _servo4_out = 0.0f;                   // output value sent to motor
 
     // parameters
     AP_Int16        _servo1_pos;                // Angular location of swash servo #1