Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AntennaTracker: Improvements to yaw slewing and ballerina mode

Browse Source
This commit is contained in:
Mike McCauley 2014-03-07 10:36:22 +10:00 committed by Andrew Tridgell
parent 33dc55714c
commit c53ad6e3d3
1 changed files with 27 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
Tools/AntennaTracker/tracking.pde
View File

@ -17,6 +17,7 @@ static struct {
*/ */
static void update_pitch_servo(float pitch) static void update_pitch_servo(float pitch)
{ {
// pitch = 0.0; // TEST
// degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal // degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal
// pitch argument 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 // servo_out is in 100ths of a degree
@ -50,6 +51,8 @@ static void update_pitch_servo(float pitch)
*/ */
static void update_yaw_servo(float yaw) static void update_yaw_servo(float yaw)
{ {
// yaw = 0.0; // TEST
// degrees(ahrs.yaw) is -180 to 180, where 0 is north // degrees(ahrs.yaw) is -180 to 180, where 0 is north
float ahrs_yaw = degrees(ahrs.yaw); float ahrs_yaw = degrees(ahrs.yaw);
// yaw argument is 0 to 360 where 0 and 360 are north // yaw argument is 0 to 360 where 0 and 360 are north
@ -84,7 +87,7 @@ static void update_yaw_servo(float yaw)
// According to the specs at https://www.servocity.com/html/hs-645mg_ultra_torque.html, // According to the specs at https://www.servocity.com/html/hs-645mg_ultra_torque.html,
// that should be 600 through 2400, but the azimuth gearing in my antenna pointer is not exactly 2:1 // that should be 600 through 2400, but the azimuth gearing in my antenna pointer is not exactly 2:1
int32_t err = (ahrs_yaw - yaw) * 100.0; int32_t err = (ahrs_yaw - yaw) * 100.0;
static int32_t last_err = 0.0; static int32_t last_err = 0;
// Correct for wrapping yaw at +-180 // Correct for wrapping yaw at +-180
// so we get the error to the _closest_ version of the target azimuth // so we get the error to the _closest_ version of the target azimuth
@ -94,47 +97,39 @@ static void update_yaw_servo(float yaw)
else if (err < -18000) else if (err < -18000)
err += 36000; err += 36000;
static int32_t slew_to = 0;
int32_t servo_err = channel_yaw.servo_out - err; // Servo position we need to get to static int8_t slew_dir = 0;
if ( !slew_to if (slew_dir == 0 &&
&& servo_err > 19000 // 10 degreee deadband channel_yaw.servo_out == 18000 &&
&& err < 0 err < -1000 && // 10 degree deadband
&& last_err > err) err < last_err)
{ {
// We need to be beyond the servo limits and the error magnitude is increasing slew_dir = -1; // Too far +ve, slew in the -ve direction
// Slew most of the way to the other side at high speed...
slew_to = servo_err - 27000;
} }
else if ( !slew_to else if (slew_dir == 0 &&
&& servo_err < -19000 // 10 degreee deadband channel_yaw.servo_out == -18000 &&
&& err > 0 err > 1000 && // 10 degree deadband
&& last_err < err) err > last_err)
{ {
// We need to be beyond the servo limits and the error magnitude is increasing slew_dir = 1; // Too far -ve, slew in the +ve direction
// Slew most of the way to the other side at high speed...
slew_to = servo_err + 27000;
} }
else if ( slew_to < 0 else if (slew_dir < 0 &&
&& err > 0 err > 0)
&& last_err < err)
{ {
// ...then let normal tracking take over slew_dir = 0;
slew_to = 0;
} }
else if ( slew_to > 0 else if (slew_dir > 0 &&
&& err < 0 err < 0)
&& last_err > err)
{ {
// ...then let normal tracking take over slew_dir = 0;
slew_to = 0;
} }
if (slew_to) last_err = err;
{ if (slew_dir > 0)
channel_yaw.servo_out = slew_to; err -= 27000;
} else if (slew_dir < 0)
else err += 27000;
{ {
// Normal tracking // Normal tracking
// You will need to tune the yaw PID to suit your antenna and servos // You will need to tune the yaw PID to suit your antenna and servos
@ -147,7 +142,6 @@ static void update_yaw_servo(float yaw)
int32_t new_servo_out = channel_yaw.servo_out - g.pidYaw2Srv.get_pid(err); int32_t new_servo_out = channel_yaw.servo_out - g.pidYaw2Srv.get_pid(err);
channel_yaw.servo_out = constrain_float(new_servo_out, -18000, 18000); channel_yaw.servo_out = constrain_float(new_servo_out, -18000, 18000);
} }
last_err = err;
channel_yaw.calc_pwm(); channel_yaw.calc_pwm();
channel_yaw.output(); channel_yaw.output();
} }