From 79b069afc3095186f3c28cb77442caa1a22c3c5d Mon Sep 17 00:00:00 2001
From: bnsgeyer <bnsgeyer@aol.com>
Date: Sat, 16 Mar 2019 08:02:59 -0400
Subject: [PATCH] AP_Motors:tradheli-swash library general clean up

---
 libraries/AP_Motors/AP_MotorsHeli_Dual.cpp   | 16 ++++------------
 libraries/AP_Motors/AP_MotorsHeli_Single.cpp |  8 ++------
 libraries/AP_Motors/AP_MotorsHeli_Single.h   |  2 +-
 libraries/AP_Motors/AP_MotorsHeli_Swash.cpp  |  7 ++-----
 libraries/AP_Motors/AP_MotorsHeli_Swash.h    |  3 +--
 5 files changed, 10 insertions(+), 26 deletions(-)

diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
index c2df98af53..9af34e8c4b 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
+++ b/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()));
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
index 7c7927497d..ad58242740 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
+++ b/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()));
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h
index 7a6e04eb3b..5425b30fc9 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Single.h
+++ b/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
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp b/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp
index 12c5635353..df2fa23508 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp
+++ b/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;
 
 }
 
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Swash.h b/libraries/AP_Motors/AP_MotorsHeli_Swash.h
index 9cd26b2d59..8a2f9f81ba 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Swash.h
+++ b/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