AP_Motors: Circular swash-plate limits for AP_MotorsHeli

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -28,23 +28,9 @@ extern const AP_HAL::HAL& hal;
 
 const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
 
-    // @Param: ROL_MAX
-    // @DisplayName: Swash Roll Angle Max
-    // @Description: Maximum roll angle of the swash plate
-    // @Range: 0 18000
-    // @Units: Centi-Degrees
-    // @Increment: 100
-    // @User: Advanced
-    AP_GROUPINFO("ROL_MAX", 1, AP_MotorsHeli, _roll_max, AP_MOTORS_HELI_SWASH_ROLL_MAX),
+    // 1 was ROL_MAX which has been replaced by CYC_MAX
 
-    // @Param: PIT_MAX
-    // @DisplayName: Swash Pitch Angle Max
-    // @Description: Maximum pitch angle of the swash plate
-    // @Range: 0 18000
-    // @Units: Centi-Degrees
-    // @Increment: 100
-    // @User: Advanced
-    AP_GROUPINFO("PIT_MAX", 2, AP_MotorsHeli, _pitch_max, AP_MOTORS_HELI_SWASH_PITCH_MAX),
+    // 2 was PIT_MAX which has been replaced by CYC_MAX
 
     // @Param: COL_MIN
     // @DisplayName: Collective Pitch Minimum
@@ -153,6 +139,15 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("RSC_POWER_HIGH", 15, AP_MotorsHeli, _rsc_power_high, AP_MOTORS_HELI_RSC_POWER_HIGH_DEFAULT),
 
+    // @Param: CYC_MAX
+    // @DisplayName: Cyclic Pitch Angle Max
+    // @Description: Maximum pitch angle of the swash plate
+    // @Range: 0 18000
+    // @Units: Centi-Degrees
+    // @Increment: 100
+    // @User: Advanced
+    AP_GROUPINFO("CYC_MAX", 16, AP_MotorsHeli, _cyclic_max, AP_MOTORS_HELI_SWASH_CYCLIC_MAX),
+
     AP_GROUPEND
 };
 

libraries/AP_Motors/AP_MotorsHeli.h

@@ -23,8 +23,7 @@
 #define AP_MOTORS_HELI_SPEED_ANALOG_SERVOS      125     // update rate for analog servos
 
 // default swash min and max angles and positions
-#define AP_MOTORS_HELI_SWASH_ROLL_MAX           2500
-#define AP_MOTORS_HELI_SWASH_PITCH_MAX          2500
+#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
@@ -196,8 +195,7 @@ protected:
     } _heliflags;
 
     // parameters
-    AP_Int16        _roll_max;                  // Maximum roll angle of the swash plate in centi-degrees
-    AP_Int16        _pitch_max;                 // Maximum pitch angle of the swash plate in centi-degrees
+    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 Assymetrical blades)

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -234,8 +234,8 @@ void AP_MotorsHeli_Single::calculate_scalars()
     _collective_range = 1000 - _collective_mid_pwm;
 
     // determine roll, pitch and collective input scaling
-    _roll_scaler = (float)_roll_max/4500.0f;
-    _pitch_scaler = (float)_pitch_max/4500.0f;
+    _roll_scaler = (float)_cyclic_max/4500.0f;
+    _pitch_scaler = (float)_cyclic_max/4500.0f;
     _collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f;
 
     // calculate factors based on swash type and servo position
@@ -353,29 +353,18 @@ void AP_MotorsHeli_Single::move_actuators(int16_t roll_out, int16_t pitch_out, i
     limit.throttle_lower = false;
     limit.throttle_upper = false;
 
-    // rescale roll_out and pitch-out into the min and max ranges to provide linear motion
+    // rescale roll_out and pitch_out into the min and max ranges to provide linear motion
     // across the input range instead of stopping when the input hits the constrain value
-    // these calculations are based on an assumption of the user specified roll_max and pitch_max
+    // these calculations are based on an assumption of the user specified cyclic_max
     // coming into this equation at 4500 or less, and based on the original assumption of the
     // total _servo_x.servo_out range being -4500 to 4500.
-    roll_out = roll_out * _roll_scaler;
-    if (roll_out < -_roll_max) {
-        roll_out = -_roll_max;
-        limit.roll_pitch = true;
-    }
-    if (roll_out > _roll_max) {
-        roll_out = _roll_max;
-        limit.roll_pitch = true;
-    }
 
-    // scale pitch and update limits
-    pitch_out = pitch_out * _pitch_scaler;
-    if (pitch_out < -_pitch_max) {
-        pitch_out = -_pitch_max;
-        limit.roll_pitch = true;
-    }
-    if (pitch_out > _pitch_max) {
-        pitch_out = _pitch_max;
+    float total_out = pythagorous2((float)pitch_out, (float)roll_out);
+
+    if (total_out > _cyclic_max) {
+        float ratio = (float)_cyclic_max / total_out;
+        roll_out *= ratio;
+        pitch_out *= ratio;
         limit.roll_pitch = true;
     }