AC_InputManager: collective blending for mode switch

libraries/AC_InputManager/AC_InputManager_Heli.cpp

@@ -107,17 +107,25 @@ float AC_InputManager_Heli::get_pilot_desired_collective(int16_t control_in)
     }
     acro_col_out = constrain_float(acro_col_out, 0.0f, 1.0f);
 
-    // ramp to and from stab col over 1/2 second
-    if (_im_flags_heli.use_stab_col && (_stab_col_ramp < 1.0f)){
-        _stab_col_ramp += 2.0f/(float)_loop_rate;
-    } else if(!_im_flags_heli.use_stab_col && (_stab_col_ramp > 0.0f)){
-        _stab_col_ramp -= 2.0f/(float)_loop_rate;
+    // ramp function
+    if (is_positive(_ramp)) {
+        float dt = 1/(float)_loop_rate;
+        // factor 2 to transition over a time span of 0.5s
+        _ramp -= 2*dt;
+        _ramp = constrain_float(_ramp, 0.0f, 1.0f);
     }
-    _stab_col_ramp = constrain_float(_stab_col_ramp, 0.0f, 1.0f);
 
-    // scale collective output smoothly between acro and stab col
+    //set Stabilize or Acro collective output
+    float new_flightmode_col_output;
+    if (_im_flags_heli.use_stab_col) {
+        new_flightmode_col_output = stab_col_out;
+    } else {
+        new_flightmode_col_output = acro_col_out;
+    }
+
+    // scale collective output smoothly between previous and current mode output
     float collective_out;
-    collective_out = (float)((1.0f-_stab_col_ramp)*acro_col_out + _stab_col_ramp*stab_col_out);
+    collective_out = new_flightmode_col_output * (1.0 - _ramp) + _ramp * _old_flightmode_col_output;
     collective_out = constrain_float(collective_out, 0.0f, 1.0f);
 
     return collective_out;

libraries/AC_InputManager/AC_InputManager_Heli.h

@@ -27,14 +27,17 @@ public:
     /* Do not allow copies */
     CLASS_NO_COPY(AC_InputManager_Heli);
 
+    //pass the last collective output from non-manual throttle mode
+    void set_last_coll_output(float collective) { _old_flightmode_col_output = collective; }
+
     // get_pilot_desired_collective - rescale's pilot collective pitch input in Stabilize and Acro modes
     float get_pilot_desired_collective(int16_t control_in);
 
     // set_use_stab_col - setter function
     void set_use_stab_col(bool use) { _im_flags_heli.use_stab_col = use; }
 
-    // set_heli_stab_col_ramp - setter function
-    void set_stab_col_ramp(float ramp) { _stab_col_ramp = constrain_float(ramp, 0.0, 1.0); }
+    // set collective_ramp - setter function
+    void set_collective_ramp(float ramp) { _ramp = constrain_float(ramp, 0.0, 1.0); }
 
     // parameter_check - returns true if input manager specific parameters are sensible, used for pre-arm check
     bool parameter_check(char* fail_msg, uint8_t fail_msg_len) const;
@@ -46,8 +49,11 @@ private:
         bool use_stab_col; // 1 if we should use Stabilise mode collective range, 0 for Acro range
     } _im_flags_heli;
 
-    //  factor used to smoothly ramp collective from Acro value to Stab-Col value
-    float _stab_col_ramp = 0;
+    // previous flight mode collective output
+    float _old_flightmode_col_output;
+
+    // ramp factor from previous mode to current mode collective output
+    float _ramp;
 
     AP_Int16        _heli_stab_col_min;             // minimum collective pitch setting at zero throttle input in Stabilize mode
     AP_Int16        _heli_stab_col_low;             // collective pitch setting at mid-low throttle input in Stabilize mode