You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
206 lines
8.3 KiB
206 lines
8.3 KiB
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
|
|
/** |
|
state of the vehicle we are tracking |
|
*/ |
|
static struct { |
|
Location location; // lat, long in degrees * 10^7; alt in meters * 100 |
|
int32_t relative_alt; // meters * 100 |
|
uint32_t last_update_us; |
|
float heading; // degrees |
|
float ground_speed; // m/s |
|
} vehicle; |
|
|
|
/** |
|
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) |
|
{ |
|
// 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); |
|
int32_t err = (ahrs_pitch - pitch) * 100.0; |
|
// Need to configure your servo so that increasing servo_out causes increase in pitch/elevation (ie pointing higher into the sky, |
|
// above the horizon. On my antenna tracker this requires the pitch/elevation servo to be reversed |
|
// param set RC2_REV -1 |
|
// |
|
// The pitch servo (RC channel 2) is configured for servo_out of -9000-0-9000 servo_out, |
|
// which will drive the servo from RC2_MIN to RC2_MAX usec pulse width. |
|
// Therefore, you must set RC2_MIN and RC2_MAX so that your servo drives the antenna altitude between -90 to 90 exactly |
|
// To drive my HS-645MG servos through their full 180 degrees of rotational range, I have to set: |
|
// param set RC2_MAX 2540 |
|
// param set RC2_MIN 640 |
|
// |
|
// You will also need to tune the pitch PID to suit your antenna and servos. I use: |
|
// PITCH2SRV_P 0.100000 |
|
// PITCH2SRV_I 0.020000 |
|
// PITCH2SRV_D 0.000000 |
|
// PITCH2SRV_IMAX 4000.000000 |
|
int32_t new_servo_out = channel_pitch.servo_out - g.pidPitch2Srv.get_pid(err); |
|
channel_pitch.servo_out = constrain_float(new_servo_out, -9000, 9000); |
|
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 pinting at the target |
|
*/ |
|
static void update_yaw_servo(float yaw) |
|
{ |
|
// degrees(ahrs.yaw) is -180 to 180, where 0 is north |
|
float ahrs_yaw = degrees(ahrs.yaw); |
|
// yaw argument is 0 to 360 where 0 and 360 are north |
|
// Make yaw -180-0-180 too |
|
if (yaw > 180) |
|
yaw = yaw - 360; |
|
|
|
// Antenna as Ballerina. Use with antenna that do not have continuously rotating servos, ie at some point in rotation |
|
// the servo limits are reached and the servo has to slew 360 degrees to the 'other side' to keep tracking. |
|
// |
|
// This algorithm accounts for the fact that the antenna mount may not be aligned with North |
|
// (in fact, any alignment is permissable), and that the alignment may change (possibly rapidly) over time |
|
// (as when the antenna is mounted on a moving, turning vehicle) |
|
// When the servo is being forced beyond its limits, it rapidly slews to the 'other side' then normal tracking takes over. |
|
// |
|
// Caution: this whole system is compromised by the fact that the ahrs_yaw reported by the compass system lags |
|
// the actual yaw by about 5 seconds, and also periodically realigns itself with about a 30 second period, |
|
// which makes it very hard to be completely sure exactly where the antenna is _really_ pointing |
|
// especially during the high speed slew. This can cause odd pointing artifacts from time to time. The best strategy is to |
|
// position and point the mount so the aircraft does not 'go behind' the antenna (if possible) |
|
// |
|
// With my antenna mount, large pwm output drives the antenna anticlockise, so need: |
|
// param set RC1_REV -1 |
|
// to reverse the servo. Yours may be different |
|
// |
|
// You MUST set RC1_MIN and RC1_MAX so that your servo drives the antenna azimuth from -180 to 180 relative |
|
// to the mount. |
|
// To drive my HS-645MG servos through their full 180 degrees of rotational range and therefore the |
|
// antenna through a full 360 degrees, I have to set: |
|
// param set RC1_MAX 2380 |
|
// param set RC1_MIN 680 |
|
// 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 |
|
int32_t err = (ahrs_yaw - yaw) * 100.0; |
|
static int32_t last_err = 0.0; |
|
|
|
// Correct for wrapping yaw at +-180 |
|
// so we get the error to the _closest_ version of the target azimuth |
|
// +ve error requires anticlockwise motion (ie towards more negative yaw) |
|
if (err > 18000) |
|
err -= 36000; |
|
else if (err < -18000) |
|
err += 36000; |
|
|
|
static int32_t slew_to = 0; |
|
|
|
int32_t servo_err = channel_yaw.servo_out - err; // Servo position we need to get to |
|
if ( !slew_to |
|
&& servo_err > 19000 // 10 degreee deadband |
|
&& err < 0 |
|
&& last_err > err) |
|
{ |
|
// We need to be beyond the servo limits and the error magnitude is increasing |
|
// Slew most of the way to the other side at high speed... |
|
slew_to = servo_err - 27000; |
|
} |
|
else if ( !slew_to |
|
&& servo_err < -19000 // 10 degreee deadband |
|
&& err > 0 |
|
&& last_err < err) |
|
{ |
|
// We need to be beyond the servo limits and the error magnitude is increasing |
|
// Slew most of the way to the other side at high speed... |
|
slew_to = servo_err + 27000; |
|
} |
|
else if ( slew_to < 0 |
|
&& err > 0 |
|
&& last_err < err) |
|
{ |
|
// ...then let normal tracking take over |
|
slew_to = 0; |
|
} |
|
else if ( slew_to > 0 |
|
&& err < 0 |
|
&& last_err > err) |
|
{ |
|
// ...then let normal tracking take over |
|
slew_to = 0; |
|
} |
|
|
|
if (slew_to) |
|
{ |
|
channel_yaw.servo_out = slew_to; |
|
} |
|
else |
|
{ |
|
// Normal tracking |
|
// You will need to tune the yaw PID to suit your antenna and servos |
|
// For my servos, suitable PID settings are: |
|
// param set YAW2SRV_P 0.1 |
|
// param set YAW2SRV_I 0.05 |
|
// param set YAW2SRV_D 0 |
|
// param set YAW2SRV_IMAX 4000 |
|
|
|
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); |
|
} |
|
last_err = err; |
|
channel_yaw.calc_pwm(); |
|
channel_yaw.output(); |
|
} |
|
|
|
|
|
/** |
|
main antenna tracking code, called at 50Hz |
|
*/ |
|
static void update_tracking(void) |
|
{ |
|
// project the vehicle position to take account of lost radio packets |
|
Location vpos = vehicle.location; |
|
float dt = (hal.scheduler->micros() - vehicle.last_update_us) * 1.0e-6f; |
|
location_update(vpos, vehicle.heading, vehicle.ground_speed * dt); |
|
|
|
// update our position if we have at least a 2D fix |
|
// REVISIT: what if we lose lock during a mission and the antenna is moving? |
|
if (g_gps->status() >= GPS::GPS_OK_FIX_2D) { |
|
current_loc.lat = g_gps->latitude; |
|
current_loc.lng = g_gps->longitude; |
|
current_loc.alt = g_gps->altitude_cm; |
|
current_loc.options = 0; // Absolute altitude |
|
} |
|
|
|
// calculate the bearing to the vehicle |
|
float bearing = get_bearing_cd(current_loc, vehicle.location) * 0.01f; |
|
float distance = get_distance(current_loc, vehicle.location); |
|
int32_t alt_diff_cm = current_loc.options & MASK_OPTIONS_RELATIVE_ALT // Do we know our absolute altitude? |
|
? (vehicle.relative_alt - current_loc.alt) |
|
: (vehicle.location.alt - current_loc.alt); // cm |
|
float pitch = degrees(atan2(alt_diff_cm/100, distance)); |
|
// update the servos |
|
update_pitch_servo(pitch); |
|
update_yaw_servo(bearing); |
|
|
|
// update nav_status for NAV_CONTROLLER_OUTPUT |
|
nav_status.bearing = bearing; |
|
nav_status.pitch = pitch; |
|
nav_status.distance = distance; |
|
} |
|
|
|
|
|
/** |
|
handle an updated position from the aircraft |
|
*/ |
|
static void tracking_update_position(const mavlink_global_position_int_t &msg) |
|
{ |
|
vehicle.location.lat = msg.lat; |
|
vehicle.location.lng = msg.lon; |
|
vehicle.location.alt = msg.alt/10; |
|
vehicle.relative_alt = msg.relative_alt/10; |
|
vehicle.heading = msg.hdg * 0.01f; |
|
vehicle.ground_speed = pythagorous2(msg.vx, msg.vy) * 0.01f; |
|
vehicle.last_update_us = hal.scheduler->micros(); |
|
} |
|
|
|
|