Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AntennaTracker: continuous rotation servos 

Global modifications for continuous rotation servos
This commit is contained in:
rcairman 2015-06-03 23:05:58 +09:00 committed by Randy Mackay
parent ac05f0f1d4
commit fba8ca3a98
4 changed files with 35 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
AntennaTracker/Parameters.cpp
View File

@ -111,7 +111,7 @@ const AP_Param::Info Tracker::var_info[] PROGMEM = {
// @Param: SERVO_TYPE // @Param: SERVO_TYPE
// @DisplayName: Type of servo system being used // @DisplayName: Type of servo system being used
// @Description: This allows selection of position servos or on/off servos // @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 // @User: Standard
GSCALAR(servo_type, "SERVO_TYPE", SERVO_TYPE_POSITION), GSCALAR(servo_type, "SERVO_TYPE", SERVO_TYPE_POSITION),

2
AntennaTracker/Tracker.h
View File

@ -218,9 +218,11 @@ private:
void init_servos(); void init_servos();
void update_pitch_servo(float pitch); void update_pitch_servo(float pitch);
void update_pitch_position_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_pitch_onoff_servo(float pitch);
void update_yaw_servo(float yaw); void update_yaw_servo(float yaw);
void update_yaw_position_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 update_yaw_onoff_servo(float yaw);
void init_tracker(); void init_tracker();
void update_notify(); void update_notify();

3
AntennaTracker/defines.h
View File

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

30
AntennaTracker/servos.cpp
View File

@ -33,6 +33,10 @@ void Tracker::update_pitch_servo(float pitch)
update_pitch_onoff_servo(pitch); update_pitch_onoff_servo(pitch);
break; break;
case SERVO_TYPE_CR:
update_pitch_cr_servo(pitch);
break;
case SERVO_TYPE_POSITION: case SERVO_TYPE_POSITION:
default: default:
update_pitch_position_servo(pitch); 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. update the yaw (azimuth) servo.
*/ */
@ -145,6 +159,10 @@ void Tracker::update_yaw_servo(float yaw)
update_yaw_onoff_servo(yaw); update_yaw_onoff_servo(yaw);
break; break;
case SERVO_TYPE_CR:
update_yaw_cr_servo(yaw);
break;
case SERVO_TYPE_POSITION: case SERVO_TYPE_POSITION:
default: default:
update_yaw_position_servo(yaw); update_yaw_position_servo(yaw);
@ -307,3 +325,15 @@ void Tracker::update_yaw_onoff_servo(float yaw)
channel_yaw.servo_out = 18000; 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);
}