From eeb544d0984416ac3a1ccca8c981075da64abfd7 Mon Sep 17 00:00:00 2001
From: Robert Lefebvre <robert.lefebvre@gmail.com>
Date: Wed, 21 Oct 2015 16:00:36 -0400
Subject: [PATCH] AP_MotorsHeli: Create SV_MAN=5=Oscillate servo setup mode.

---
 libraries/AP_Motors/AP_MotorsHeli.cpp        |  4 ++
 libraries/AP_Motors/AP_MotorsHeli.h          |  5 ++
 libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 52 ++++++++++++++++++++
 libraries/AP_Motors/AP_MotorsHeli_Single.h   |  3 ++
 4 files changed, 64 insertions(+)

diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp
index 50de250a8d..db82793b89 100644
--- a/libraries/AP_Motors/AP_MotorsHeli.cpp
+++ b/libraries/AP_Motors/AP_MotorsHeli.cpp
@@ -277,6 +277,10 @@ void AP_MotorsHeli::output_disarmed()
             _throttle_control_input = 0;
             _yaw_control_input = -4500;
             break;
+        case SERVO_CONTROL_MODE_MANUAL_OSCILLATE:
+            // use servo_test function from child classes
+            servo_test();
+            break;
         default:
             // no manual override
             break;
diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h
index 2074500d7e..bcd10c639e 100644
--- a/libraries/AP_Motors/AP_MotorsHeli.h
+++ b/libraries/AP_Motors/AP_MotorsHeli.h
@@ -153,6 +153,10 @@ public:
     // reset_radio_passthrough used to reset all radio inputs to center
     void reset_radio_passthrough();
 
+    // servo_test - move servos through full range of movement
+    // to be overloaded by child classes, different vehicle types would have different movement patterns
+    virtual void servo_test() = 0;
+
     // output - sends commands to the motors
     void    output();
 
@@ -163,6 +167,7 @@ public:
         SERVO_CONTROL_MODE_MANUAL_MAX,
         SERVO_CONTROL_MODE_MANUAL_CENTER,
         SERVO_CONTROL_MODE_MANUAL_MIN,
+        SERVO_CONTROL_MODE_MANUAL_OSCILLATE,
     };
 
     // supports_yaw_passthrough
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
index 97b275315f..1bce86cae2 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
+++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
@@ -478,3 +478,55 @@ void AP_MotorsHeli_Single::write_aux(int16_t servo_out)
     _servo_aux.calc_pwm();
     hal.rcout->write(AP_MOTORS_HELI_SINGLE_AUX, _servo_aux.radio_out);
 }
+
+// servo_test - move servos through full range of movement
+void AP_MotorsHeli_Single::servo_test()
+{
+    static float oscillate_angle = 0.0f;
+    static float servo_test_cycle_time = 0.0f;
+    static float collective_test = 0.0f;
+    static float roll_test = 0.0f;
+    static float pitch_test = 0.0f;
+    static float yaw_test = 0.0f;
+
+    servo_test_cycle_time += 1.0f / _loop_rate;
+
+    if ((servo_test_cycle_time >= 0.0f && servo_test_cycle_time < 0.5f)||                                   // Tilt swash back
+        (servo_test_cycle_time >= 6.0f && servo_test_cycle_time < 6.5f)){
+        pitch_test += (4500 / (_loop_rate/2));
+        oscillate_angle += 8 * M_PI_F / _loop_rate;
+        yaw_test = 2250 * sinf(oscillate_angle);
+    } else if ((servo_test_cycle_time >= 0.5f && servo_test_cycle_time < 4.5f)||                            // Roll swash around
+               (servo_test_cycle_time >= 6.5f && servo_test_cycle_time < 10.5f)){
+        oscillate_angle += M_PI_F / (2 * _loop_rate);
+        roll_test = 4500 * sinf(oscillate_angle);
+        pitch_test = 4500 * cosf(oscillate_angle);
+        yaw_test = 4500 * sinf(oscillate_angle);
+    } else if ((servo_test_cycle_time >= 4.5f && servo_test_cycle_time < 5.0f)||                            // Return swash to level
+               (servo_test_cycle_time >= 10.5f && servo_test_cycle_time < 11.0f)){
+        pitch_test -= (4500 / (_loop_rate/2));
+        oscillate_angle += 8 * M_PI_F / _loop_rate;
+        yaw_test = 2250 * sinf(oscillate_angle);
+    } else if (servo_test_cycle_time >= 5.0f && servo_test_cycle_time < 6.0f){                              // Raise swash to top
+        collective_test += (1000 / _loop_rate);
+        oscillate_angle += 2 * M_PI_F / _loop_rate;
+        yaw_test = 4500 * sinf(oscillate_angle);
+    } else if (servo_test_cycle_time >= 11.0f && servo_test_cycle_time < 12.0f){                            // Lower swash to bottom
+        collective_test -= (1000 / _loop_rate);
+        oscillate_angle += 2 * M_PI_F / _loop_rate;
+        yaw_test = 4500 * sinf(oscillate_angle);
+    } else {                                                                                                // reset cycle
+        servo_test_cycle_time = 0.0f;
+        oscillate_angle = 0.0f;
+        collective_test = 0.0f;
+        roll_test = 0.0f;
+        pitch_test = 0.0f;
+        yaw_test = 0.0f;
+    }
+
+    // over-ride servo commands to move servos through defined ranges
+    _throttle_control_input = collective_test;
+    _roll_control_input = roll_test;
+    _pitch_control_input = pitch_test;
+    _yaw_control_input = yaw_test;
+}
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h
index a984663538..80ccc5be19 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Single.h
+++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h
@@ -120,6 +120,9 @@ public:
     void set_delta_phase_angle(int16_t angle);
 
     void set_acro_tail(bool set) { _acro_tail = set; }
+
+    // servo_test - move servos through full range of movement
+    void servo_test();
     
     // var_info
     static const struct AP_Param::GroupInfo var_info[];