From 704a7204122679a4f818585ae2fb7738d3ed6915 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 6 Oct 2014 14:56:25 +0900
Subject: [PATCH] Tracker: move servo init to servos.pde

---
 AntennaTracker/servos.pde | 17 +++++++++++++++++
 AntennaTracker/system.pde | 11 ++---------
 2 files changed, 19 insertions(+), 9 deletions(-)

diff --git a/AntennaTracker/servos.pde b/AntennaTracker/servos.pde
index 19ed81ce1c..b115f663e1 100644
--- a/AntennaTracker/servos.pde
+++ b/AntennaTracker/servos.pde
@@ -4,6 +4,23 @@
  * servos.pde - code to move pitch and yaw servos to attain a target heading or pitch
  */
 
+// init_servos - initialises the servos
+static void init_servos()
+{
+    // setup antenna control PWM channels
+    channel_yaw.set_angle(g.yaw_range * 100/2);        // yaw range is +/- (YAW_RANGE parameter/2) converted to centi-degrees
+    channel_pitch.set_angle(g.pitch_range * 100/2);    // pitch range is +/- (PITCH_RANGE parameter/2) converted to centi-degrees
+
+    // move servos to mid position
+    channel_yaw.output_trim();
+    channel_pitch.output_trim();
+
+    // initialise output to servos
+    channel_yaw.calc_pwm();
+    channel_pitch.calc_pwm();
+}
+
+
 /**
    update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the
    requested pitch, so the board (and therefore the antenna) will be pointing at the target
diff --git a/AntennaTracker/system.pde b/AntennaTracker/system.pde
index 2104fc0745..a2d8c153b4 100644
--- a/AntennaTracker/system.pde
+++ b/AntennaTracker/system.pde
@@ -74,15 +74,8 @@ static void init_tracker()
     hal.uartB->set_blocking_writes(false);
     hal.uartC->set_blocking_writes(false);
 
-    // setup antenna control PWM channels
-    channel_yaw.set_angle(18000); // Yaw is expected to drive antenna azimuth -180-0-180
-    channel_pitch.set_angle(9000); // Pitch is expected to drive elevation -90-0-90
-
-    channel_yaw.output_trim();
-    channel_pitch.output_trim();
-
-    channel_yaw.calc_pwm();
-    channel_pitch.calc_pwm();
+    // initialise servos
+    init_servos();
 
     // use given start positions - useful for indoor testing, and
     // while waiting for GPS lock