AP_MotorsHeli: Make servo linearisation generic

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -592,5 +592,10 @@ bool AP_MotorsHeli_Dual::arming_checks(size_t buflen, char *buffer) const
         return false;
     }
 
+    // Run swashplate specific checks
+    if (!_swashplate1.arming_checks(buflen, buffer) || !_swashplate2.arming_checks(buflen, buffer)) {
+        return false;
+    }
+
     return true;
 }

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -647,6 +647,11 @@ bool AP_MotorsHeli_Single::arming_checks(size_t buflen, char *buffer) const
         return false;
     }
 
+    // Run swashplate specific checks
+    if (!_swashplate.arming_checks(buflen, buffer)) {
+        return false;
+    }
+
     return true;
 }
 

libraries/AP_Motors/AP_MotorsHeli_Swash.cpp

@@ -42,7 +42,6 @@ const AP_Param::GroupInfo AP_MotorsHeli_Swash::var_info[] = {
     // @Description: This linearizes the swashplate servo's mechanical output to account for nonlinear output due to arm rotation.  This requires a specific setup procedure to work properly.  The servo arm must be centered on the mechanical throw at the servo trim position and the servo trim position kept as close to 1500 as possible. Leveling the swashplate can only be done through the pitch links.  See the ardupilot wiki for more details on setup.
     // @Values: 0:Disabled,1:Enabled
     // @User: Standard
-    AP_GROUPINFO("LIN_SVO", 3, AP_MotorsHeli_Swash, _linear_swash_servo, 0),
 
     // @Param: H3_ENABLE
     // @DisplayName: Enable Generic H3 Swashplate Settings
@@ -83,6 +82,14 @@ const AP_Param::GroupInfo AP_MotorsHeli_Swash::var_info[] = {
     // @User: Advanced
     // @Increment: 1
     AP_GROUPINFO("H3_PHANG", 8, AP_MotorsHeli_Swash, _phase_angle, 0),
+
+    // @Param: LIN_SV_ANG
+    // @DisplayName: Linearize Swashplate Servo Mechanical Throw
+    // @Description: Set 0 to disable. Set max angle range of servo horn from level. This linearizes the swashplate servo's mechanical output to account for nonlinear output due to arm rotation. This requires a specific setup procedure to work properly. The servo arm must be centered on the mechanical throw at the servo trim position and the servo trim position kept as close to 1500 as possible. Leveling the swashplate can only be done through the pitch links. See the ardupilot wiki for more details on setup.
+    // @Range: 0 60
+    // @Units: deg
+    // @User: Standard
+    AP_GROUPINFO("LIN_SV_ANG", 9, AP_MotorsHeli_Swash, _linear_swash_servo_ang_deg, 0),
 
     AP_GROUPEND
 };
@@ -103,7 +110,6 @@ void AP_MotorsHeli_Swash::configure()
 
     _swash_type = static_cast<SwashPlateType>(_swashplate_type.get());
     _collective_direction = static_cast<CollectiveDirection>(_swash_coll_dir.get());
-    _make_servo_linear = _linear_swash_servo != 0;
     enable.set(_swash_type == SWASHPLATE_TYPE_H3);
 
     calculate_roll_pitch_collective_factors();
@@ -233,22 +239,24 @@ void AP_MotorsHeli_Swash::calculate(float roll, float pitch, float collective)
         // rescale from -1..1, so we can use the pwm calc that includes trim
         _output[i] = 2.0f * _output[i] - 1.0f;
 
-        if (_make_servo_linear) {
-            _output[i] = get_linear_servo_output(_output[i]);
-        }
-
+        // If servo linearisation param is set we linearise the output
+        linearise_servo_output(_output[i]);
     }
 }
 
 // set_linear_servo_out - sets swashplate servo output to be linear
-float AP_MotorsHeli_Swash::get_linear_servo_output(float input) const
+void AP_MotorsHeli_Swash::linearise_servo_output(float& input) const
 {
+    if (_linear_swash_servo_ang_deg.get() <= 0) {
+        // Don't apply linearisation
+        return;
+    }
 
     input = constrain_float(input, -1.0f, 1.0f);
 
-    //servo output is calculated by normalizing input to 50 deg arm rotation as full input for a linear throw
-    return safe_asin(0.766044f * input) * 1.145916;
-
+    // servo output is calculated by normalizing input to max servo horn rotation angle
+    float servo_ang_rads = radians(_linear_swash_servo_ang_deg.get());
+    input = safe_asin(sinf(servo_ang_rads) * input) * 1/servo_ang_rads;
 }
 
 // Output calculated values to servos
@@ -283,3 +291,16 @@ uint32_t AP_MotorsHeli_Swash::get_output_mask() const
     }
     return mask;
 }
+
+bool AP_MotorsHeli_Swash::arming_checks(size_t buflen, char *buffer) const
+{
+    // Sanity check swash linearisation angle range
+    if (_linear_swash_servo_ang_deg.get() >= 90.0) {
+        if (_instance == 1) {
+            hal.util->snprintf(buffer, buflen, "H_SW_LIN_SV_ANG > 90 out of range");
+        } else {
+            hal.util->snprintf(buffer, buflen, "H_SW%u_LIN_SV_ANG > 90 out of range", _instance);
+        }
+        return false;
+    }
+}

libraries/AP_Motors/AP_MotorsHeli_Swash.h

@@ -39,13 +39,16 @@ class AP_MotorsHeli_Swash {
     // Get function output mask
     uint32_t get_output_mask() const;
 
+    // Swashplate specific arming checks
+    bool arming_checks(size_t buflen, char *buffer) const;
+
     // var_info
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
 
     // linearize mechanical output of swashplate servo
-    float get_linear_servo_output(float input) const;
+    void linearise_servo_output(float& input) const;
 
     // CCPM Mixers - calculate mixing scale factors by swashplate type
     void calculate_roll_pitch_collective_factors();
@@ -67,7 +70,6 @@ class AP_MotorsHeli_Swash {
     // Currently configured setup
     SwashPlateType       _swash_type;                 // Swashplate type
     CollectiveDirection  _collective_direction;       // Collective control direction, normal or reversed
-    bool                 _make_servo_linear;          // Sets servo output to be linearized
 
     // Internal variables
     bool                 _enabled[_max_num_servos];                 // True if this output servo is enabled
@@ -80,7 +82,7 @@ class AP_MotorsHeli_Swash {
     // parameters
     AP_Int8  _swashplate_type;                   // Swash Type Setting
     AP_Int8  _swash_coll_dir;                    // Collective control direction, normal or reversed
-    AP_Int8  _linear_swash_servo;                // linearize swashplate output
+    AP_Float  _linear_swash_servo_ang_deg;       // Swashplate servo horn max range level, used to linearize swashplate output
     AP_Int8  enable;
     AP_Int16 _servo1_pos;                        // servo1 azimuth position on swashplate with front of heli being 0 deg
     AP_Int16 _servo2_pos;                        // servo2 azimuth position on swashplate with front of heli being 0 deg