AP_MotorsHeli: Make swash linearisation more generic

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -592,5 +592,15 @@ bool AP_MotorsHeli_Dual::arming_checks(size_t buflen, char *buffer) const
         return false;
     }
 
+    // Sanity check swash linearierisation angle range
+    if (_swashplate1.get_lin_angle()) {
+        hal.util->snprintf(buffer, buflen, "H_SW_LIN_SV_ANG > 90 out of range");
+        return false;
+    }
+    if (_swashplate2.get_lin_angle()) {
+        hal.util->snprintf(buffer, buflen, "H_SW2_LIN_SV_ANG > 90 out of range");
+        return false;
+    }
+
     return true;
 }

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -647,6 +647,12 @@ bool AP_MotorsHeli_Single::arming_checks(size_t buflen, char *buffer) const
         return false;
     }
 
+    // Sanity check swash linearierisation angle range
+    if (_swashplate.get_lin_angle()) {
+        hal.util->snprintf(buffer, buflen, "H_SW_LIN_SV_ANG > 90 out of range");
+        return false;
+    }
+
     return true;
 }
 

libraries/AP_Motors/AP_MotorsHeli_Swash.cpp

@@ -37,12 +37,13 @@ const AP_Param::GroupInfo AP_MotorsHeli_Swash::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("COL_DIR", 2, AP_MotorsHeli_Swash, _swash_coll_dir, COLLECTIVE_DIRECTION_NORMAL),
 
-    // @Param: LIN_SVO
+    // @Param: LIN_SV_ANG
     // @DisplayName: Linearize Swashplate Servo Mechanical Throw
-    // @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
+    // @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_SVO", 3, AP_MotorsHeli_Swash, _linear_swash_servo, 0),
+    AP_GROUPINFO("LIN_SV_ANG", 3, AP_MotorsHeli_Swash, _linear_swash_servo_ang_deg, 0),
 
     // @Param: H3_ENABLE
     // @DisplayName: Enable Generic H3 Swashplate Settings
@@ -103,7 +104,7 @@ 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;
+    _make_servo_linear = _linear_swash_servo_ang_deg > 0;
     enable.set(_swash_type == SWASHPLATE_TYPE_H3);
 
     calculate_roll_pitch_collective_factors();
@@ -243,12 +244,10 @@ void AP_MotorsHeli_Swash::calculate(float roll, float pitch, float collective)
 // set_linear_servo_out - sets swashplate servo output to be linear
 float AP_MotorsHeli_Swash::get_linear_servo_output(float input) const
 {
-
     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
+    return safe_asin(sinf(radians(_linear_swash_servo_ang_deg.get())) * input) * 1/radians(_linear_swash_servo_ang_deg.get());
 }
 
 // Output calculated values to servos

libraries/AP_Motors/AP_MotorsHeli_Swash.h

@@ -39,6 +39,8 @@ class AP_MotorsHeli_Swash {
     // Get function output mask
     uint32_t get_output_mask() const;
 
+    float get_lin_angle(void) const { return _linear_swash_servo_ang_deg.get(); }
+
     // var_info
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -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