AntennaTracker: continuous rotation servos

Global modifications for continuous rotation servos

AntennaTracker/Parameters.cpp

@@ -111,7 +111,7 @@ const AP_Param::Info Tracker::var_info[] PROGMEM = {
     // @Param: SERVO_TYPE
     // @DisplayName: Type of servo system being used
     // @Description: This allows selection of position servos or on/off servos
-    // @Values: 0:Position,1:OnOff
+    // @Values: 0:Position,1:OnOff,2:ContinuousRotation
     // @User: Standard
     GSCALAR(servo_type,          "SERVO_TYPE",   SERVO_TYPE_POSITION),
 

AntennaTracker/Tracker.h

@@ -218,9 +218,11 @@ private:
     void init_servos();
     void update_pitch_servo(float pitch);
     void update_pitch_position_servo(float pitch);
+    void update_pitch_cr_servo(float pitch);
     void update_pitch_onoff_servo(float pitch);
     void update_yaw_servo(float yaw);
     void update_yaw_position_servo(float yaw);
+    void update_yaw_cr_servo(float yaw);
     void update_yaw_onoff_servo(float yaw);
     void init_tracker();
     void update_notify();

AntennaTracker/defines.h

@@ -22,7 +22,8 @@ enum ControlMode {
 
 enum ServoType {
     SERVO_TYPE_POSITION=0,
-    SERVO_TYPE_ONOFF=1
+    SERVO_TYPE_ONOFF=1,
+    SERVO_TYPE_CR=2
 };
 
 #endif // _DEFINES_H

AntennaTracker/servos.cpp

@@ -33,6 +33,10 @@ void Tracker::update_pitch_servo(float pitch)
         update_pitch_onoff_servo(pitch);
         break;
 
+    case SERVO_TYPE_CR:
+        update_pitch_cr_servo(pitch);
+        break;
+
     case SERVO_TYPE_POSITION:
     default:
         update_pitch_position_servo(pitch);
@@ -135,6 +139,16 @@ void Tracker::update_pitch_onoff_servo(float pitch)
     }
 }
 
+/**
+   update the pitch for continuous rotation servo
+*/
+void Tracker::update_pitch_cr_servo(float pitch)
+{
+    float ahrs_pitch = degrees(ahrs.pitch);
+    float err_cd = (ahrs_pitch - pitch) * 100.0f;
+    channel_pitch.servo_out = g.pidPitch2Srv.get_pid(err_cd);
+}
+
 /**
    update the yaw (azimuth) servo.
  */
@@ -145,6 +159,10 @@ void Tracker::update_yaw_servo(float yaw)
         update_yaw_onoff_servo(yaw);
         break;
 
+    case SERVO_TYPE_CR:
+        update_yaw_cr_servo(yaw);
+        break;
+
     case SERVO_TYPE_POSITION:
     default:
         update_yaw_position_servo(yaw);
@@ -307,3 +325,15 @@ void Tracker::update_yaw_onoff_servo(float yaw)
         channel_yaw.servo_out = 18000;
     }
 }
+
+/**
+   update the yaw continuous rotation servo
+ */
+void Tracker::update_yaw_cr_servo(float yaw)
+{
+    int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
+    float yaw_cd = wrap_180_cd_float(yaw*100.0f);
+    float err_cd = wrap_180_cd((float)ahrs_yaw_cd - yaw_cd);
+
+    channel_yaw.servo_out = g.pidYaw2Srv.get_pid(err_cd);
+}