AP_Motors:tradheli-swash library general clean up

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -271,13 +271,9 @@ void AP_MotorsHeli_Dual::calculate_scalars()
 
     // configure swashplate 1 and update scalars
     if (_swashplate1_type == SWASHPLATE_TYPE_H3) {
-        if (_swashplate1.get_enable() == 0) {
-            _swashplate1.set_enable(1);
-        }
+        _swashplate1.set_enable(1);
     } else {
-        if (_swashplate1.get_enable() == 1) {
-            _swashplate1.set_enable(0);
-        }
+        _swashplate1.set_enable(0);
     }
     _swashplate1.set_swash_type(static_cast<SwashPlateType>(_swashplate1_type.get()));
     _swashplate1.set_collective_direction(static_cast<CollectiveDirection>(_swash1_coll_dir.get()));
@@ -285,13 +281,9 @@ void AP_MotorsHeli_Dual::calculate_scalars()
 
     // configure swashplate 2 and update scalars
     if (_swashplate2_type == SWASHPLATE_TYPE_H3) {
-        if (_swashplate2.get_enable() == 0) {
-            _swashplate2.set_enable(1);
-        }
+        _swashplate2.set_enable(1);
     } else {
-        if (_swashplate2.get_enable() == 1) {
-            _swashplate2.set_enable(0);
-        }
+        _swashplate2.set_enable(0);
     }
     _swashplate2.set_swash_type(static_cast<SwashPlateType>(_swashplate2_type.get()));
     _swashplate2.set_collective_direction(static_cast<CollectiveDirection>(_swash2_coll_dir.get()));

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -251,13 +251,9 @@ void AP_MotorsHeli_Single::calculate_scalars()
 
     // configure swashplate and update scalars
     if (_swashplate_type == SWASHPLATE_TYPE_H3) {
-        if (_swashplate.get_enable() == 0) {
-            _swashplate.set_enable(1);
-        }
+        _swashplate.set_enable(1);
     } else {
-        if (_swashplate.get_enable() == 1) {
-            _swashplate.set_enable(0);
-        }
+        _swashplate.set_enable(0);
     }
     _swashplate.set_swash_type(static_cast<SwashPlateType>(_swashplate_type.get()));
     _swashplate.set_collective_direction(static_cast<CollectiveDirection>(_swash_coll_dir.get()));

libraries/AP_Motors/AP_MotorsHeli_Single.h

@@ -132,7 +132,7 @@ protected:
     float _servo5_out = 0.0f;                   // output value sent to motor
 
     // parameters
-    AP_Int8         _swash_coll_dir;      // Collective control direction, normal or reversed
+    AP_Int8         _swash_coll_dir;            // Collective control direction, normal or reversed
     AP_Int16        _tail_type;                 // Tail type used: Servo, Servo with external gyro, direct drive variable pitch or direct drive fixed pitch
     AP_Int8         _swashplate_type;           // Swash Type Setting
     AP_Int16        _ext_gyro_gain_std;         // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro

libraries/AP_Motors/AP_MotorsHeli_Swash.cpp

@@ -180,14 +180,11 @@ float AP_MotorsHeli_Swash::get_servo_out(int8_t ch_num, float pitch, float roll,
 // set_linear_servo_out - sets swashplate servo output to be linear
 float AP_MotorsHeli_Swash::get_linear_servo_output(float input) const
 {
-    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;
+    //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;
 
 }
 

libraries/AP_Motors/AP_MotorsHeli_Swash.h

@@ -52,9 +52,8 @@ public:
 
     // allow parameters to be enabled
     void set_enable(int8_t setenable) {enable = setenable; }
-    int8_t get_enable() { return enable; }
 
-    // 
+    // get_phase_angle - returns the rotor phase angle which is used to remove coupling between pitch and roll axes
     int16_t get_phase_angle() const { return _phase_angle; }
 
     // var_info