AntennaTracker: added SCAN mode

this is used to test calibrate the compass, and to test tracking PIDs

Tools/AntennaTracker/AntennaTracker.pde

@@ -109,6 +109,8 @@ static struct {
     bool manual_control_yaw:1;
     bool manual_control_pitch:1;
     bool need_altitude_calibration:1;
+    bool scan_reverse_pitch:1;
+    bool scan_reverse_yaw:1;
 } nav_status;
 
 static uint32_t start_time_ms;

Tools/AntennaTracker/GCS_Mavlink.pde

@@ -44,6 +44,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
     case STOP:
         break;
 
+    case SCAN:
     case AUTO:
         base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED |
             MAV_MODE_FLAG_STABILIZE_ENABLED;
@@ -897,6 +898,7 @@ mission_failed:
         switch (packet.custom_mode) {
         case MANUAL:
         case STOP:
+        case SCAN:
         case AUTO:
             set_mode((enum ControlMode)packet.custom_mode);
             break;

Tools/AntennaTracker/Parameters.h

@@ -79,6 +79,7 @@ public:
         k_param_startup_delay,
         k_param_BoardConfig,
         k_param_gps,
+        k_param_scan_speed,
 
         k_param_channel_yaw = 200,
         k_param_channel_pitch,
@@ -109,6 +110,7 @@ public:
     AP_Float yaw_slew_time;
     AP_Float pitch_slew_time;
     AP_Float min_reverse_time;
+    AP_Float scan_speed;
 
     AP_Float start_latitude;
     AP_Float start_longitude;

Tools/AntennaTracker/Parameters.pde

@@ -75,6 +75,14 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Range: 0 20
     GSCALAR(pitch_slew_time,        "PITCH_SLEW_TIME",  2),
 
+    // @Param: SCAN_SPEED
+    // @DisplayName: Speed at which to rotate in scan mode
+    // @Description: This controls how rapidly the tracker will move the servos in SCAN mode
+    // @Units: degrees/second
+    // @Increment: 1
+    // @Range: 0 100
+    GSCALAR(scan_speed,             "SCAN_SPEED",      5),
+
     // @Param: MIN_REVERSE_TIME
     // @DisplayName: Minimum time to apply a yaw reversal
     // @Description: When the tracker detects it has reached the limit of servo movement in yaw it will reverse and try moving to the other extreme of yaw. This parameter controls the minimum time it should reverse for. It is used to cope with trackers that have a significant lag in movement to ensure they do move all the way around.

Tools/AntennaTracker/defines.h

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

Tools/AntennaTracker/system.pde

@@ -228,6 +228,7 @@ static void set_mode(enum ControlMode mode)
 	switch (control_mode) {
     case AUTO:
     case MANUAL:
+    case SCAN:
         arm_servos();
         break;
 

Tools/AntennaTracker/tracking.pde

@@ -207,6 +207,38 @@ static void update_manual(void)
     channel_pitch.output();
 }
 
+/*
+  control servos for SCAN mode
+ */
+static void update_scan(void)
+{    
+    if (!nav_status.manual_control_yaw) {
+        float yaw_delta = g.scan_speed * 0.02f;
+        nav_status.bearing   += yaw_delta   * (nav_status.scan_reverse_yaw?-1:1);
+        if (nav_status.bearing < 0 && nav_status.scan_reverse_yaw) {
+            nav_status.scan_reverse_yaw = false;
+        }
+        if (nav_status.bearing > 360 && !nav_status.scan_reverse_yaw) {
+            nav_status.scan_reverse_yaw = true;
+        }
+        nav_status.bearing = constrain_float(nav_status.bearing, 0, 360);
+    }
+
+    if (!nav_status.manual_control_pitch) {
+        float pitch_delta = g.scan_speed * 0.02f;
+        nav_status.pitch += pitch_delta * (nav_status.scan_reverse_pitch?-1:1);
+        if (nav_status.pitch < -90 && nav_status.scan_reverse_pitch) {
+            nav_status.scan_reverse_pitch = false;
+        }
+        if (nav_status.pitch > 90 && !nav_status.scan_reverse_pitch) {
+            nav_status.scan_reverse_pitch = true;
+        }
+        nav_status.pitch = constrain_float(nav_status.pitch, -90, 90);
+    }
+
+    update_auto();
+}
+
 
 /**
   main antenna tracking code, called at 50Hz
@@ -230,10 +262,10 @@ static void update_tracking(void)
     float pitch    = degrees(atan2f(nav_status.altitude_difference, distance));
 
     // update nav_status for NAV_CONTROLLER_OUTPUT
-    if (!nav_status.manual_control_yaw) {
+    if (control_mode != SCAN && !nav_status.manual_control_yaw) {
         nav_status.bearing = bearing;
     }
-    if (!nav_status.manual_control_pitch) {
+    if (control_mode != SCAN && !nav_status.manual_control_pitch) {
         nav_status.pitch = pitch;
     }
     nav_status.distance = distance;
@@ -247,6 +279,10 @@ static void update_tracking(void)
         update_manual();
         break;
 
+    case SCAN:
+        update_scan();
+        break;
+
     case STOP:
     case INITIALISING:
         break;