AP_Motors: Tradheli - support for linear servo output for swashplates

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -193,6 +193,11 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("RSC_THRCRV_100", 24, AP_MotorsHeli, _rsc_thrcrv[4], AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT),
 
+    // @Param: LIN_SW_SERVO
+    // @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.
+    // @User: Standard
+    AP_GROUPINFO("LIN_SW_SERVO", 25, AP_MotorsHeli, _linear_swash_servo, 0),
     AP_GROUPEND
 };
 

libraries/AP_Motors/AP_MotorsHeli.h

@@ -213,6 +213,7 @@ protected:
     AP_Int16        _rsc_thrcrv[5];             // throttle value sent to throttle servo at 0, 25, 50, 75 and 100 percent collective
     AP_Int16        _rsc_slewrate;              // throttle slew rate (percentage per second)
     AP_Int8         _servo_test;                // sets number of cycles to test servo movement on bootup
+    AP_Int8         _linear_swash_servo;         // linearize swashplate output
 
     // internal variables
     float           _collective_mid_pct = 0.0f;      // collective mid parameter value converted to 0 ~ 1 range

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -268,6 +268,7 @@ void AP_MotorsHeli_Single::calculate_scalars()
     _swashplate.set_swash_type(static_cast<SwashPlateType>((uint8_t)_swashplate_type));
     _swashplate.set_collective_direction(static_cast<CollectiveDirection>((uint8_t)_swash_coll_dir));
     _swashplate.calculate_roll_pitch_collective_factors();
+    _swashplate.set_linear_servo_out(_linear_swash_servo);
 
     // send setpoints to main rotor controller and trigger recalculation of scalars
     _main_rotor.set_control_mode(static_cast<RotorControlMode>(_rsc_mode.get()));

libraries/AP_Motors/AP_MotorsHeli_Swash.cpp

@@ -178,6 +178,23 @@ float AP_MotorsHeli_Swash::get_servo_out(int8_t CH_num, float pitch, float roll,
     // rescale from -1..1, so we can use the pwm calc that includes trim
     servo = 2.0f * servo - 1.0f;
 
+    if (_make_servo_linear == 1) {
+        servo = get_linear_servo_output(servo);
+    }
+
     return servo;
 }
 
+float AP_MotorsHeli_Swash::get_linear_servo_output(float input)
+{
+    float ret;
+
+    input = constrain_float(input, -1.0f, 1.0f);
+
+    //servo output is normalized to 0.866 for a linear throw
+    ret = asin(0.766044f * input) * 1.145916;
+
+    return ret;
+
+}
+

libraries/AP_Motors/AP_MotorsHeli_Swash.h

@@ -49,6 +49,11 @@ public:
     void set_servo3_pos(int16_t servo_pos) { _servo3_pos = servo_pos; }
     void set_servo4_pos(int16_t servo_pos) { _servo4_pos = servo_pos; }
 
+    // set_linear_servo_out - sets swashplate servo output to be linear
+    void set_linear_servo_out(int8_t linear_servo) { _make_servo_linear = linear_servo; }
+
+    //linearize mechanical output of swashplate servo
+    float get_linear_servo_output(float input);
 
 private:
     // internal variables
@@ -62,6 +67,7 @@ private:
     int16_t         _servo2_pos;
     int16_t         _servo3_pos;
     int16_t         _servo4_pos;
+    int8_t          _make_servo_linear;
 
 };
 class SwashInt16Param {