AP_Motors: coll setup uses actual blade pitch angle

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -44,14 +44,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("COL_MAX", 4, AP_MotorsHeli, _collective_max, AP_MOTORS_HELI_COLLECTIVE_MAX),
 
-    // @Param: COL_MID
-    // @DisplayName: Zero-Thrust Collective Pitch 
-    // @Description: Swash servo position in PWM microseconds corresponding to zero collective pitch (or zero lift for Asymmetrical blades)
-    // @Range: 1000 2000
-    // @Units: PWM
-    // @Increment: 1
-    // @User: Standard
-    AP_GROUPINFO("COL_MID", 5, AP_MotorsHeli, _collective_mid, AP_MOTORS_HELI_COLLECTIVE_MID),
+    // index 5 was COL_MID. Do not use this index in the future.
 
     // @Param: SV_MAN
     // @DisplayName: Manual Servo Mode
@@ -117,6 +110,42 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("OPTIONS", 28, AP_MotorsHeli, _heli_options, (uint8_t)HeliOption::USE_LEAKY_I),
 
+    // @Param: COL_MIN_DEG
+    // @DisplayName: Minimum Collective Blade Pitch Angle
+    // @Description: Minimum collective blade pitch angle in deg that corresponds to the PWM set for minimum collective pitch (H_COL_MIN).
+    // @Range: -20 0
+    // @Units: deg
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("COL_MIN_DEG", 29, AP_MotorsHeli, _collective_min_deg, AP_MOTORS_HELI_COLLECTIVE_MIN_DEG),
+
+    // @Param: COL_MAX_DEG
+    // @DisplayName: Maximum Collective Blade Pitch Angle
+    // @Description: Maximum collective blade pitch angle in deg that corresponds to the PWM set for maximum collective pitch (H_COL_MAX).
+    // @Range: 5 20
+    // @Units: deg
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("COL_MAX_DEG", 30, AP_MotorsHeli, _collective_max_deg, AP_MOTORS_HELI_COLLECTIVE_MAX_DEG),
+
+    // @Param: COL_ZERO_THRST
+    // @DisplayName: Zero-Thrust Collective Pitch 
+    // @Description: Zero thrust blade collective pitch in degrees.
+    // @Range: -5 0
+    // @Units: deg
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("COL_ZERO_THRST", 31, AP_MotorsHeli, _collective_zero_thrst_deg, 0.0f),
+
+    // @Param: COL_LAND_MIN
+    // @DisplayName: Minimum Landed Collective Blade Pitch
+    // @Description: Minimum Landed collective blade pitch in degrees for non-manual collective modes (i.e. modes that use altitude hold).
+    // @Range: -5 0
+    // @Units: deg
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("COL_LAND_MIN", 32, AP_MotorsHeli, _collective_land_min_deg, AP_MOTORS_HELI_COLLECTIVE_LAND_MIN),
+
     AP_GROUPEND
 };
 
@@ -253,7 +282,7 @@ void AP_MotorsHeli::output_disarmed()
                 // fixate mid collective
                 _roll_in = 0.0f;
                 _pitch_in = 0.0f;
-                _throttle_filter.reset(_collective_mid_pct);
+                _throttle_filter.reset(_collective_zero_pct);
                 _yaw_in = 0.0f;
                 break;
             case SERVO_CONTROL_MODE_MANUAL_MAX:
@@ -516,10 +545,10 @@ void AP_MotorsHeli::update_throttle_hover(float dt)
 {
     if (_collective_hover_learn != HOVER_LEARN_DISABLED) {
 
-        // Don't let _collective_hover go below H_COLL_MID
+        // Don't let _collective_hover go below H_COLL_ZERO_THRST
         float curr_collective = get_throttle();
-        if (curr_collective < _collective_mid_pct) {
-            curr_collective = _collective_mid_pct;
+        if (curr_collective < _collective_zero_pct) {
+            curr_collective = _collective_zero_pct;
         }
 
         // we have chosen to constrain the hover collective to be within the range reachable by the third order expo polynomial.
@@ -539,7 +568,7 @@ void AP_MotorsHeli::save_params_on_disarm()
 // updates the takeoff collective flag
 void AP_MotorsHeli::update_takeoff_collective_flag(float coll_out)
 {
-    if (coll_out > _collective_mid_pct + 0.5f * (_collective_hover - _collective_mid_pct)) {
+    if (coll_out > _collective_zero_pct + 0.5f * (_collective_hover - _collective_zero_pct)) {
         _heliflags.takeoff_collective = true;
     } else {
         _heliflags.takeoff_collective = false;

libraries/AP_Motors/AP_MotorsHeli.h

@@ -18,11 +18,14 @@
 #define AP_MOTORS_HELI_SWASH_CYCLIC_MAX         2500
 #define AP_MOTORS_HELI_COLLECTIVE_MIN           1250
 #define AP_MOTORS_HELI_COLLECTIVE_MAX           1750
-#define AP_MOTORS_HELI_COLLECTIVE_MID           1500
 #define AP_MOTORS_HELI_COLLECTIVE_HOVER_DEFAULT 0.5f  // the estimated hover throttle, 0 ~ 1
 #define AP_MOTORS_HELI_COLLECTIVE_HOVER_TC      10.0f // time constant used to update estimated hover throttle, 0 ~ 1
 #define AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN     0.3f  // minimum possible hover throttle
 #define AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX     0.8f // maximum possible hover throttle
+#define AP_MOTORS_HELI_COLLECTIVE_MIN_DEG      -10.0f // minimum collective blade pitch angle in deg
+#define AP_MOTORS_HELI_COLLECTIVE_MAX_DEG       10.0f // maximum collective blade pitch angle in deg
+#define AP_MOTORS_HELI_COLLECTIVE_LAND_MIN      -2.0f // minimum landed collective blade pitch angle in deg for modes using althold
+
 
 // flybar types
 #define AP_MOTORS_HELI_NOFLYBAR                 0
@@ -253,15 +256,19 @@ protected:
     AP_Int16        _cyclic_max;                // Maximum cyclic angle of the swash plate in centi-degrees
     AP_Int16        _collective_min;            // Lowest possible servo position for the swashplate
     AP_Int16        _collective_max;            // Highest possible servo position for the swashplate
-    AP_Int16        _collective_mid;            // Swash servo position corresponding to zero collective pitch (or zero lift for Asymmetrical blades)
     AP_Int8         _servo_mode;                // Pass radio inputs directly to servos during set-up through mission planner
     AP_Int8         _servo_test;                // sets number of cycles to test servo movement on bootup
     AP_Float        _collective_hover;          // estimated collective required to hover throttle in the range 0 ~ 1
     AP_Int8         _collective_hover_learn;    // enable/disabled hover collective learning
     AP_Int8         _heli_options;              // bitmask for optional features
+    AP_Float        _collective_zero_thrst_deg; // Zero thrust blade collective pitch in degrees
+    AP_Float        _collective_land_min_deg;   // Minimum Landed collective blade pitch in degrees for non-manual collective modes (i.e. modes that use altitude hold)
+    AP_Float        _collective_max_deg;        // Maximum collective blade pitch angle in deg that corresponds to the PWM set for maximum collective pitch (H_COL_MAX)
+    AP_Float        _collective_min_deg;        // Minimum collective blade pitch angle in deg that corresponds to the PWM set for minimum collective pitch (H_COL_MIN)
 
     // internal variables
-    float           _collective_mid_pct = 0.0f;      // collective mid parameter value converted to 0 ~ 1 range
+    float           _collective_zero_pct = 0.0f;      // collective mid parameter value converted to 0 ~ 1 range
+    float           _collective_land_min_pct = 0.0f;      // collective mid parameter value converted to 0 ~ 1 range
     uint8_t         _servo_test_cycle_counter = 0;   // number of test cycles left to run after bootup
 
     motor_frame_type _frame_type;

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -77,14 +77,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("COL2_MAX", 17, AP_MotorsHeli_Dual, _collective2_max, AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX),
 
-    // @Param: COL2_MID
-    // @DisplayName: Swash 2 Zero-Thrust Collective Pitch
-    // @Description: Swash servo position in PWM microseconds corresponding to zero collective pitch for the rear swashplate (or zero lift for Asymmetrical blades)
-    // @Range: 1000 2000
-    // @Units: PWM
-    // @Increment: 1
-    // @User: Standard
-    AP_GROUPINFO("COL2_MID", 18, AP_MotorsHeli_Dual, _collective2_mid, AP_MOTORS_HELI_DUAL_COLLECTIVE2_MID),
+    // Indice 18 was used by COL2_MID and should not be used
 
     // Indice 19 was used by COL_CTRL_DIR and should not be used
 
@@ -382,12 +375,16 @@ void AP_MotorsHeli_Dual::calculate_scalars()
         _collective2_max = AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX;
     }
 
-    _collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max);
-    _collective2_mid = constrain_int16(_collective2_mid, _collective2_min, _collective2_max);
+    _collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg);
+
+    // calculate collective zero thrust point as a number from 0 to 1
+    _collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
+    _collective2_zero_pct = _collective_zero_pct;
+
+    _collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg);
 
-    // calculate collective mid point as a number from 0 to 1000
-    _collective_mid_pct = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min));
-    _collective2_mid_pct = ((float)(_collective2_mid-_collective2_min))/((float)(_collective2_max-_collective2_min));
+    // calculate collective land min point as a number from 0 to 1
+    _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
 
     // configure swashplate 1 and update scalars
     _swashplate1.configure();
@@ -568,13 +565,13 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
     }
 
     // ensure not below landed/landing collective
-    if (_heliflags.landing_collective && collective_out < _collective_mid_pct) {
-        collective_out = _collective_mid_pct;
+    if (_heliflags.landing_collective && collective_out < _collective_land_min_pct) {
+        collective_out = _collective_land_min_pct;
         limit.throttle_lower = true;
     }
 
     // updates below mid collective flag
-    if (collective_out <= _collective_mid_pct) {
+    if (collective_out <= _collective_land_min_pct) {
         _heliflags.below_mid_collective = true;
     } else {
         _heliflags.below_mid_collective = false;
@@ -586,7 +583,7 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
     // Set rear collective to midpoint if required
     float collective2_out = collective_out;
     if (_servo_mode == SERVO_CONTROL_MODE_MANUAL_CENTER) {
-        collective2_out = _collective2_mid_pct;
+        collective2_out = _collective2_zero_pct;
     }
 
     // if servo output not in manual mode, process pre-compensation factors
@@ -598,7 +595,7 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
             // for intermeshing, reverse yaw in negative collective region and smoothen transition near zero collective
             if (_yaw_rev_expo > 0.01f) {
                 // yaw_compensation range: (-1,1) S-shaped curve (Logistic Model) 1/(1 + e^kt)
-                yaw_compensation = 1.0f - (2.0f / (1.0f + powf(2.7182818f , _yaw_rev_expo * (collective_out-_collective_mid_pct))));
+                yaw_compensation = 1.0f - (2.0f / (1.0f + powf(2.7182818f , _yaw_rev_expo * (collective_out-_collective_zero_pct))));
                 yaw_out = yaw_out * yaw_compensation;
             }
         } else {

libraries/AP_Motors/AP_MotorsHeli_Dual.h

@@ -31,7 +31,6 @@
 // default collective min, max and midpoints for the rear swashplate
 #define AP_MOTORS_HELI_DUAL_COLLECTIVE2_MIN 1250
 #define AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX 1750
-#define AP_MOTORS_HELI_DUAL_COLLECTIVE2_MID 1500
 
 /// @class AP_MotorsHeli_Dual
 class AP_MotorsHeli_Dual : public AP_MotorsHeli {
@@ -130,7 +129,6 @@ protected:
     // parameters
     AP_Int16        _collective2_min;               // Lowest possible servo position for the rear swashplate
     AP_Int16        _collective2_max;               // Highest possible servo position for the rear swashplate
-    AP_Int16        _collective2_mid;               // Swash servo position corresponding to zero collective pitch for the rear swashplate (or zero lift for Asymmetrical blades)
     AP_Int8         _dual_mode;                     // which dual mode the heli is
     AP_Float        _dcp_scaler;                    // scaling factor applied to the differential-collective-pitch
     AP_Float        _dcp_yaw_effect;                // feed-forward compensation to automatically add yaw input when differential collective pitch is applied.
@@ -139,5 +137,5 @@ protected:
     AP_Float        _yaw_rev_expo;                  // yaw reverser smoothing exponent, for intermeshing mode only.
 
     // internal variables
-    float           _collective2_mid_pct = 0.0f;      // collective mid parameter value for rear swashplate converted to 0 ~ 1 range
+    float _collective2_zero_pct;
 };

libraries/AP_Motors/AP_MotorsHeli_Quad.cpp

@@ -147,10 +147,15 @@ void AP_MotorsHeli_Quad::calculate_scalars()
         _collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX;
     }
 
-    _collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max);
+    _collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg);
 
-    // calculate collective mid point as a number from 0 to 1000
-    _collective_mid_pct = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min));
+    // calculate collective zero thrust point as a number from 0 to 1
+    _collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
+
+    _collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg);
+
+    // calculate collective land min point as a number from 0 to 1
+    _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
 
     // calculate factors based on swash type and servo position
     calculate_roll_pitch_collective_factors();
@@ -238,13 +243,13 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
     }
 
     // ensure not below landed/landing collective
-    if (_heliflags.landing_collective && collective_out < _collective_mid_pct) {
-        collective_out = _collective_mid_pct;
+    if (_heliflags.landing_collective && collective_out < _collective_land_min_pct) {
+        collective_out = _collective_land_min_pct;
         limit.throttle_lower = true;
     }
 
     // updates below mid collective flag
-    if (collective_out <= _collective_mid_pct) {
+    if (collective_out <= _collective_land_min_pct) {
         _heliflags.below_mid_collective = true;
     } else {
         _heliflags.below_mid_collective = false;
@@ -263,7 +268,7 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
     _main_rotor.set_collective(fabsf(collective_out));
 
     // rescale collective for overhead calc
-    collective_out -= _collective_mid_pct;
+    collective_out -= _collective_zero_pct;
 
     // reserve some collective for attitude control
     collective_out *= collective_range;
@@ -304,7 +309,7 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
 
     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
         // scale output to 0 to 1
-        _out[i] += _collective_mid_pct;
+        _out[i] += _collective_zero_pct;
         // scale output to -1 to 1 for servo output
         _out[i] = _out[i] * 2.0f - 1.0f;
     }

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -319,15 +319,20 @@ void AP_MotorsHeli_Single::calculate_armed_scalars()
 // calculate_scalars - recalculates various scalers used.
 void AP_MotorsHeli_Single::calculate_scalars()
 {
-    // range check collective min, max and mid
+    // range check collective min, max and zero
     if( _collective_min >= _collective_max ) {
         _collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN;
         _collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX;
     }
-    _collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max);
+    _collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg);
 
-    // calculate collective mid point as a number from 0 to 1
-    _collective_mid_pct = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min));
+    // calculate collective zero thrust point as a number from 0 to 1
+    _collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
+
+    _collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg);
+
+    // calculate collective land min point as a number from 0 to 1
+    _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
 
     // configure swashplate and update scalars
     _swashplate.configure();
@@ -441,14 +446,14 @@ 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 && !_heliflags.in_autorotation) {
-        collective_out = _collective_mid_pct;
+    if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_heliflags.in_autorotation) {
+        collective_out = _collective_land_min_pct;
         limit.throttle_lower = true;
 
     }
 
     // updates below mid collective flag
-    if (collective_out <= _collective_mid_pct) {
+    if (collective_out <= _collective_land_min_pct) {
         _heliflags.below_mid_collective = true;
     } else {
         _heliflags.below_mid_collective = false;
@@ -466,7 +471,7 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
             // sanity check collective_yaw_effect
             _collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE);
             // the 4.5 scaling factor is to bring the values in line with previous releases
-            yaw_offset = _collective_yaw_effect * fabsf(collective_out - _collective_mid_pct) / 4.5f;
+            yaw_offset = _collective_yaw_effect * fabsf(collective_out - _collective_zero_pct) / 4.5f;
         }
     } else {
         yaw_offset = 0.0f;