AP_Motors: Added support for autorotation

libraries/AP_Motors/AP_MotorsHeli.h

@@ -124,6 +124,12 @@ class AP_MotorsHeli : public AP_Motors {
     // support passing init_targets_on_arming flag to greater code
     bool init_targets_on_arming() const override { return _heliflags.init_targets_on_arming; }
 
+    // set_in_autorotation - allows main code to set when aircraft is in autorotation.
+    void set_in_autorotation(bool autorotation) { _heliflags.in_autorotation = autorotation; }
+
+    // set_enable_bailout - allows main code to set when RSC can immediately ramp engine instantly
+    void set_enable_bailout(bool bailout) { _heliflags.enable_bailout = bailout; }
+
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -191,6 +197,8 @@ class AP_MotorsHeli : public AP_Motors {
         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
         uint8_t save_rsc_mode           : 1;    // used to determine the rsc mode needs to be saved while disarmed
+        uint8_t in_autorotation         : 1;    // true if aircraft is in autorotation
+        uint8_t enable_bailout          : 1;    // true if allowing RSC to quickly ramp up engine
     } _heliflags;
 
     // parameters

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -332,6 +332,9 @@ void AP_MotorsHeli_Dual::calculate_armed_scalars()
         _main_rotor._rsc_mode.save();
         _heliflags.save_rsc_mode = false;
     }
+
+    // set bailout ramp time
+    _main_rotor.use_bailout_ramp_time(_heliflags.enable_bailout);
 }
 
 // calculate_scalars

libraries/AP_Motors/AP_MotorsHeli_Quad.cpp

@@ -120,6 +120,9 @@ void AP_MotorsHeli_Quad::calculate_armed_scalars()
         _main_rotor._rsc_mode.save();
         _heliflags.save_rsc_mode = false;
     }
+
+    // set bailout ramp time
+    _main_rotor.use_bailout_ramp_time(_heliflags.enable_bailout);
 }
 
 // calculate_scalars

libraries/AP_Motors/AP_MotorsHeli_RSC.cpp

@@ -294,9 +294,12 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
 // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output
 void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input, float dt)
 {
-    // sanity check ramp time
-    if (_ramp_time <= 0) {
-        _ramp_time = 1;
+    int8_t ramp_time;
+    // sanity check ramp time and enable bailout if set
+    if (_use_bailout_ramp || _ramp_time <= 0) {
+        ramp_time = 1;
+    } else {
+        ramp_time = _ramp_time;
     }
 
     // ramp output upwards towards target
@@ -306,7 +309,7 @@ void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input, float dt)
             _rotor_ramp_output = _rotor_runup_output;
         }
         // ramp up slowly to target
-        _rotor_ramp_output += (dt / _ramp_time);
+        _rotor_ramp_output += (dt / ramp_time);
         if (_rotor_ramp_output > rotor_ramp_input) {
             _rotor_ramp_output = rotor_ramp_input;
         }

libraries/AP_Motors/AP_MotorsHeli_RSC.h

@@ -17,6 +17,7 @@
 // default main rotor ramp up time in seconds
 #define AP_MOTORS_HELI_RSC_RAMP_TIME            1       // 1 second to ramp output to main rotor ESC to setpoint
 #define AP_MOTORS_HELI_RSC_RUNUP_TIME           10      // 10 seconds for rotor to reach full speed
+#define AP_MOTORS_HELI_RSC_BAILOUT_TIME         1       // time in seconds to ramp motors when bailing out of autorotation
 
 // Throttle Curve Defaults
 #define AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT     25
@@ -116,6 +117,9 @@ class AP_MotorsHeli_RSC {
     // set_collective. collective for throttle curve calculation
     void        set_collective(float collective) { _collective_in = collective; }
 
+    // use bailout ramp time
+    void        use_bailout_ramp_time(bool enable) { _use_bailout_ramp = enable; }
+
     // output - update value to send to ESC/Servo
     void        output(RotorControlState state);
 
@@ -149,6 +153,7 @@ class AP_MotorsHeli_RSC {
     float           _rotor_rpm;                   // rotor rpm from speed sensor for governor
     float           _governor_output;             // governor output for rotor speed control
     bool            _governor_engage;             // RSC governor status flag for soft-start
+    bool            _use_bailout_ramp;            // true if allowing RSC to quickly ramp up engine
 
     // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output
     void            update_rotor_ramp(float rotor_ramp_input, float dt);

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -283,6 +283,10 @@ void AP_MotorsHeli_Single::calculate_armed_scalars()
         _main_rotor._rsc_mode.save();
         _heliflags.save_rsc_mode = false;
     }
+
+    // set bailout ramp time
+    _main_rotor.use_bailout_ramp_time(_heliflags.enable_bailout);
+    _tail_rotor.use_bailout_ramp_time(_heliflags.enable_bailout);
 }
 
 // calculate_scalars - recalculates various scalers used.
@@ -413,13 +417,13 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
     }
 
     // ensure not below landed/landing collective
-    if (_heliflags.landing_collective && collective_out < _collective_mid_pct) {
+    if (_heliflags.landing_collective && collective_out < _collective_mid_pct && !_heliflags.in_autorotation) {
         collective_out = _collective_mid_pct;
         limit.throttle_lower = true;
     }
 
-    // if servo output not in manual mode, process pre-compensation factors
-    if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED) {
+    // if servo output not in manual mode and heli is not in autorotation, process pre-compensation factors
+    if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED && !_heliflags.in_autorotation) {
         // rudder feed forward based on collective
         // the feed-forward is not required when the motor is stopped or at idle, and thus not creating torque
         // also not required if we are using external gyro

libraries/AP_Motors/AP_Motors_Class.cpp

@@ -176,3 +176,10 @@ void AP_Motors::add_motor_num(int8_t motor_num)
         }
     }
 }
+
+namespace AP {
+    AP_Motors *motors()
+    {
+        return AP_Motors::get_singleton();
+    }
+}

libraries/AP_Motors/AP_Motors_Class.h

@@ -260,3 +260,7 @@ class AP_Motors {
 private:
     static AP_Motors *_singleton;
 };
+
+namespace AP {
+    AP_Motors *motors();
+};