AP_Motors: add parameters for HELI_DUAL

- add COL2_MIN/MID/MAX parameters that control limits of rear swashplate
- output collective_mid correctly for rear swashplate when servo is in manual mode

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -145,6 +145,34 @@ const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("RSC_PWM_REV", 15, AP_MotorsHeli_Dual, _rotor._pwm_rev, 1),
 
+
+    // @Param: COL_MIN
+    // @DisplayName: Collective Pitch Minimum for rear swashplate
+    // @Description: Lowest possible servo position for the rear swashplate
+    // @Range: 1000 2000
+    // @Units: PWM
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("COL2_MIN", 16, AP_MotorsHeli_Dual, _collective2_min, AP_MOTORS_HELI_DUAL_COLLECTIVE2_MIN),
+
+    // @Param: COL_MAX
+    // @DisplayName: Collective Pitch Maximum for rear swashplate
+    // @Description: Highest possible servo position for the rear swashplate
+    // @Range: 1000 2000
+    // @Units: PWM
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("COL2_MAX", 17, AP_MotorsHeli_Dual, _collective2_max, AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX),
+
+    // @Param: COL2_MID
+    // @DisplayName: Collective Pitch Mid-Point for rear swashplate
+    // @Description: Swash servo position 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),
+
     AP_GROUPEND
 };
 
@@ -282,10 +310,20 @@ void AP_MotorsHeli_Dual::calculate_scalars()
         _collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN;
         _collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX;
     }
+
+
+    // range check collective min, max and mid for rear swashplate
+    if( _collective2_min >= _collective2_max ) {
+        _collective2_min = AP_MOTORS_HELI_DUAL_COLLECTIVE2_MIN;
+        _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);
 
     // 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 factors based on swash type and servo position
     calculate_roll_pitch_collective_factors();
@@ -463,16 +501,27 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
         limit.throttle_upper = true;
     }
 
+    // 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;
+    }
+
+
     // ensure not below landed/landing collective
     if (_heliflags.landing_collective && collective_out < (_land_collective_min/1000.0f)) {
         collective_out = _land_collective_min/1000.0f;
         limit.throttle_lower = true;
     }
 
-    // scale collective pitch
+    // scale collective pitch for front swashplate (servos 1,2,3)
     float collective_scaler = ((float)(_collective_max-_collective_min))/1000.0f;
     float collective_out_scaled = collective_out * collective_scaler + (_collective_min - 1000)/1000.0f;
 
+    // scale collective pitch for rear swashplate (servos 4,5,6)
+    float collective2_scaler = ((float)(_collective2_max-_collective2_min))/1000.0f;
+    float collective2_out_scaled = collective2_out * collective2_scaler + (_collective2_min - 1000)/1000.0f;
+
     // feed power estimate into main rotor controller
     // ToDo: add main rotor cyclic power?
     _rotor.set_motor_load(fabsf(collective_out - _collective_mid_pct));
@@ -481,9 +530,9 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
     float servo1_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out + _yawFactor[CH_1] * yaw_out)/0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
     float servo2_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out + _yawFactor[CH_2] * yaw_out)/0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
     float servo3_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out + _yawFactor[CH_3] * yaw_out)/0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
-    float servo4_out = (_rollFactor[CH_4] * roll_out + _pitchFactor[CH_4] * pitch_out + _yawFactor[CH_4] * yaw_out)/0.45f + _collectiveFactor[CH_4] * collective_out_scaled;
-    float servo5_out = (_rollFactor[CH_5] * roll_out + _pitchFactor[CH_5] * pitch_out + _yawFactor[CH_5] * yaw_out)/0.45f + _collectiveFactor[CH_5] * collective_out_scaled;
-    float servo6_out = (_rollFactor[CH_6] * roll_out + _pitchFactor[CH_6] * pitch_out + _yawFactor[CH_6] * yaw_out)/0.45f + _collectiveFactor[CH_6] * collective_out_scaled;
+    float servo4_out = (_rollFactor[CH_4] * roll_out + _pitchFactor[CH_4] * pitch_out + _yawFactor[CH_4] * yaw_out)/0.45f + _collectiveFactor[CH_4] * collective2_out_scaled;
+    float servo5_out = (_rollFactor[CH_5] * roll_out + _pitchFactor[CH_5] * pitch_out + _yawFactor[CH_5] * yaw_out)/0.45f + _collectiveFactor[CH_5] * collective2_out_scaled;
+    float servo6_out = (_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
     servo1_out = 2*servo1_out - 1;

libraries/AP_Motors/AP_MotorsHeli_Dual.h

@@ -35,6 +35,11 @@
 // maximum number of swashplate servos
 #define AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS    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 {
 public:
@@ -47,6 +52,7 @@ public:
         AP_Param::setup_object_defaults(this, var_info);
     };
 
+
     // set_update_rate - set update rate to motors
     void set_update_rate( uint16_t speed_hz ) override;
 
@@ -114,6 +120,9 @@ protected:
     float _pitch_test = 0.0f;                       // over-ride for pitch output, used by servo_test function
 
     // 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_Int16        _servo1_pos;                    // angular location of swash servo #1
     AP_Int16        _servo2_pos;                    // angular location of swash servo #2
     AP_Int16        _servo3_pos;                    // angular location of swash servo #3
@@ -135,6 +144,7 @@ protected:
     SRV_Channel    *_swash_servo_6;
 
     // internal variables
+    float           _collective2_mid_pct = 0.0f;      // collective mid parameter value for rear swashplate converted to 0 ~ 1 range
     float           _rollFactor[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
     float           _pitchFactor[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
     float           _collectiveFactor[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];