Tracker: add SERVO_TEST mode

tracker switches into this mode when it receives a do-set-servo command
from the GCS.  In this mode the tracker simply moves the servo to the
specified pwm.

AntennaTracker/GCS_Mavlink.pde

@@ -42,6 +42,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
         break;
 
     case SCAN:
+    case SERVO_TEST:
     case AUTO:
         base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED |
             MAV_MODE_FLAG_STABILIZE_ENABLED;
@@ -572,7 +573,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     result = MAV_RESULT_UNSUPPORTED;
                 }
             break;
-            
+
             case MAV_CMD_DO_SET_MODE:
                 switch ((uint16_t)packet.param1) {
                     case MAV_MODE_MANUAL_ARMED:
@@ -592,6 +593,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 }
                 break;
 
+            case MAV_CMD_DO_SET_SERVO:
+                if (servo_test_set_servo(packet.param1, packet.param2)) {
+                    result = MAV_RESULT_ACCEPTED;
+                }
+                break;
+
                 // mavproxy/mavutil sends this when auto command is entered 
             case MAV_CMD_MISSION_START:
                 set_mode(AUTO);

AntennaTracker/control_servo_test.pde

@@ -0,0 +1,50 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+ * control_servo_test.pde - GCS controlled servo test mode
+ */
+
+/*
+ * update_servo_test - runs the servo test controller
+ *  called at 50hz while control_mode is 'SERVO_TEST' mode
+ *  tracker switches into this mode if it ever receives a do-set-servo command from the GCS
+ */
+static void update_servo_test(void)
+{
+    // do nothing
+}
+
+/*
+ * servo_test_set_servo - sets the yaw or pitch servo pwm directly
+ *  servo_num are 1 for yaw, 2 for pitch
+ */
+static bool servo_test_set_servo(uint8_t servo_num, uint16_t pwm)
+{
+    // convert servo_num from 1~2 to 0~1 range
+    servo_num--;
+
+    // exit immediately if servo_num is invalid
+    if (servo_num != CH_YAW && servo_num != CH_PITCH) {
+        return false;
+    }
+
+    // ensure we are in servo test mode
+    if (control_mode != SERVO_TEST) {
+        set_mode(SERVO_TEST);
+    }
+
+    // set yaw servo pwm and send output to servo
+    if (servo_num == CH_YAW) {
+        channel_yaw.radio_out = constrain_int16(pwm, channel_yaw.radio_min, channel_yaw.radio_max);
+        channel_yaw.output();
+    }
+
+    // set pitch servo pwm and send output to servo
+    if (servo_num == CH_PITCH) {
+        channel_pitch.radio_out = constrain_int16(pwm, channel_pitch.radio_min, channel_pitch.radio_max);
+        channel_pitch.output();
+    }
+
+    // return success
+    return true;
+}

AntennaTracker/defines.h

@@ -19,6 +19,7 @@ enum ControlMode {
     MANUAL=0,
     STOP=1,
     SCAN=2,
+    SERVO_TEST=3,
     AUTO=10,
     INITIALISING=16
 };

AntennaTracker/system.pde

@@ -179,6 +179,7 @@ static void set_mode(enum ControlMode mode)
     case AUTO:
     case MANUAL:
     case SCAN:
+    case SERVO_TEST:
         arm_servos();
         break;
 
@@ -198,6 +199,7 @@ static bool mavlink_set_mode(uint8_t mode)
     case AUTO:
     case MANUAL:
     case SCAN:
+    case SERVO_TEST:
     case STOP:
         set_mode((enum ControlMode)mode);
         return true;

AntennaTracker/tracking.pde

@@ -100,6 +100,7 @@ static void update_tracking(void)
         update_scan();
         break;
 
+    case SERVO_TEST:
     case STOP:
     case INITIALISING:
         break;