AntennaTracker: support on/off servos

these servos are either moving at a constant rate, or off. To really
move them accurately we need to know about this and not use a PID.

This doesn't yet support ballerina, but could do with a bit more work.

Tools/AntennaTracker/Parameters.h

@@ -81,6 +81,11 @@ public:
         k_param_gps,
         k_param_scan_speed,
         k_param_proxy_mode,
+        k_param_servo_type,
+        k_param_onoff_yaw_rate,
+        k_param_onoff_pitch_rate,
+        k_param_onoff_yaw_mintime,
+        k_param_onoff_pitch_mintime,
 
         k_param_channel_yaw = 200,
         k_param_channel_pitch,
@@ -118,6 +123,11 @@ public:
 
     AP_Float startup_delay;
     AP_Int8  proxy_mode;
+    AP_Int8  servo_type;
+    AP_Float onoff_yaw_rate;
+    AP_Float onoff_pitch_rate;
+    AP_Float onoff_yaw_mintime;
+    AP_Float onoff_pitch_mintime;
 
     // Waypoints
     //

Tools/AntennaTracker/Parameters.pde

@@ -123,6 +123,44 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Range: 0 1
     GSCALAR(proxy_mode,          "PROXY_MODE",   0),
 
+    // @Param: SERVO_TYPE
+    // @DisplayName: Type of servo system being used
+    // @Description: This allows selection of position servos or on/off servos
+    // @Values: Position:0,OnOff:1
+    GSCALAR(servo_type,          "SERVO_TYPE",   SERVO_TYPE_POSITION),
+
+    // @Param: ONOFF_YAW_RATE
+    // @DisplayName: Yaw rate for on/off servos
+    // @Description: Rate of change of yaw in degrees/second for on/off servos
+    // @Units: degrees/second
+    // @Increment: 0.1
+    // @Range: 0 50
+    GSCALAR(onoff_yaw_rate,      "ONOFF_YAW_RATE", 9.0f),
+
+    // @Param: ONOFF_PITCH_RATE
+    // @DisplayName: Pitch rate for on/off servos
+    // @Description: Rate of change of pitch in degrees/second for on/off servos
+    // @Units: degrees/second
+    // @Increment: 0.1
+    // @Range: 0 50
+    GSCALAR(onoff_pitch_rate,      "ONOFF_PITCH_RATE", 1.0f),
+
+    // @Param: ONOFF_YAW_MINT
+    // @DisplayName: Yaw minimum movement time
+    // @Description: Minimum amount of time in seconds to move in yaw
+    // @Units: seconds
+    // @Increment: 0.01
+    // @Range: 0 2
+    GSCALAR(onoff_yaw_mintime,     "ONOFF_YAW_MINT", 0.1f),
+
+    // @Param: ONOFF_PITCH_MINT
+    // @DisplayName: Pitch minimum movement time
+    // @Description: Minimim amount of time in seconds to move in pitch
+    // @Units: seconds
+    // @Increment: 0.01
+    // @Range: 0 2
+    GSCALAR(onoff_pitch_mintime,   "ONOFF_PITCH_MINT", 0.1f),
+
     // barometer ground calibration. The GND_ prefix is chosen for
     // compatibility with previous releases of ArduPlane
     // @Group: GND_

Tools/AntennaTracker/defines.h

@@ -61,5 +61,10 @@ enum ControlMode {
     INITIALISING=16
 };
 
+enum ServoType {
+    SERVO_TYPE_POSITION=0,
+    SERVO_TYPE_ONOFF=1
+};
+
 #endif // _DEFINES_H
 

Tools/AntennaTracker/tracking.pde

@@ -15,7 +15,7 @@ static struct {
    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
  */
-static void update_pitch_servo(float pitch)
+static void update_pitch_position_servo(float pitch)
 {
     // degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal
     // pitch argument is -90 to 90, where 0 is horizontal
@@ -52,17 +52,63 @@ static void update_pitch_servo(float pitch)
                                         channel_pitch.servo_out + max_change);
     }
     channel_pitch.servo_out = new_servo_out;
+}
 
+
+/**
+   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
+ */
+static void update_pitch_onoff_servo(float pitch)
+{
+    // degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal
+    // pitch argument is -90 to 90, where 0 is horizontal
+    // servo_out is in 100ths of a degree
+    float ahrs_pitch = degrees(ahrs.pitch);
+    float err = ahrs_pitch - pitch; 
+
+    float acceptable_error = g.onoff_pitch_rate * g.onoff_pitch_mintime;
+    if (fabsf(err) < acceptable_error) {
+        channel_pitch.servo_out = 0;
+    } else if (err > 0) {
+        // positive error means we are pointing too high, so push the
+        // servo down
+        channel_pitch.servo_out = -9000;        
+    } else {
+        // negative error means we are pointing too low, so push the
+        // servo up
+        channel_pitch.servo_out = 9000;
+    }
+}
+
+
+/**
+   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
+ */
+static void update_pitch_servo(float pitch)
+{
+    switch ((enum ServoType)g.servo_type.get()) {
+    case SERVO_TYPE_ONOFF:
+        update_pitch_onoff_servo(pitch);
+        break;
+
+    case SERVO_TYPE_POSITION:
+    default:
+        update_pitch_position_servo(pitch);
+        break;
+    }
     channel_pitch.calc_pwm();
     channel_pitch.output();
 }
 
+
 /**
    update the yaw (azimuth) servo. The aim is to drive the boards ahrs
    yaw to the requested yaw, so the board (and therefore the antenna)
    will be pointing at the target
  */
-static void update_yaw_servo(float yaw)
+static void update_yaw_position_servo(float yaw)
 {
     int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
     int32_t yaw_cd   = wrap_180_cd(yaw*100);
@@ -176,7 +222,50 @@ static void update_yaw_servo(float yaw)
     }
 
     channel_yaw.servo_out = new_servo_out;
+}
 
+
+/**
+   update the yaw (azimuth) servo. The aim is to drive the boards ahrs
+   yaw to the requested yaw, so the board (and therefore the antenna)
+   will be pointing at the target
+ */
+static void update_yaw_onoff_servo(float yaw)
+{
+    int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
+    int32_t yaw_cd   = wrap_180_cd(yaw*100);
+    int32_t err_cd = wrap_180_cd(ahrs_yaw_cd - yaw_cd);
+    float err = err_cd * 0.01f;
+    
+    float acceptable_error = g.onoff_yaw_rate * g.onoff_yaw_mintime;
+    if (fabsf(err) < acceptable_error) {
+        channel_yaw.servo_out = 0;
+    } else if (err > 0) {
+        // positive error means we are clockwise of the target, so
+        // move anti-clockwise
+        channel_yaw.servo_out = -18000;        
+    } else {
+        // negative error means we are anti-clockwise of the target, so
+        // move clockwise
+        channel_yaw.servo_out = 18000;        
+    }
+}
+
+/**
+   update the yaw (azimuth) servo. 
+ */
+static void update_yaw_servo(float pitch)
+{
+    switch ((enum ServoType)g.servo_type.get()) {
+    case SERVO_TYPE_ONOFF:
+        update_yaw_onoff_servo(pitch);
+        break;
+
+    case SERVO_TYPE_POSITION:
+    default:
+        update_yaw_position_servo(pitch);
+        break;
+    }
     channel_yaw.calc_pwm();
     channel_yaw.output();
 }