diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index 4f93b3dc15..4978ce9a8d 100644 --- a/AntennaTracker/Parameters.cpp +++ b/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), diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index 2f305feb91..7a009bccb0 100644 --- a/AntennaTracker/Tracker.h +++ b/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(); diff --git a/AntennaTracker/defines.h b/AntennaTracker/defines.h index 8e26c4d304..3a695f9863 100644 --- a/AntennaTracker/defines.h +++ b/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 diff --git a/AntennaTracker/servos.cpp b/AntennaTracker/servos.cpp index fdd4d1284e..4f5747d7ea 100644 --- a/AntennaTracker/servos.cpp +++ b/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); +}