From ae1fbdb68af9ca754d897162c3f10f2887abec79 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 c47a73ce0c..e9a8fb7ba9 100644
--- a/libraries/AP_Motors/AP_MotorsHeli.cpp
+++ b/libraries/AP_Motors/AP_MotorsHeli.cpp
@@ -276,6 +276,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 c64b89a5de..fcdbc8b42f 100644
--- a/libraries/AP_Motors/AP_MotorsHeli.h
+++ b/libraries/AP_Motors/AP_MotorsHeli.h
@@ -162,6 +162,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();
 
@@ -172,6 +176,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 2cd6848d94..5dce973c97 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
+++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
@@ -469,3 +469,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 c9eba1ea7a..4555896c34 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Single.h
+++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h
@@ -121,6 +121,9 @@ public:
     // set_delta_phase_angle for setting variable phase angle compensation and force
     // recalculation of collective factors
     void set_delta_phase_angle(int16_t angle);
+
+    // servo_test - move servos through full range of movement
+    void servo_test();
     
     // var_info
     static const struct AP_Param::GroupInfo var_info[];