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.
81 lines
2.3 KiB
81 lines
2.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; |
|
uint32_t last_update_us; |
|
float heading; // degrees |
|
float ground_speed; // m/s |
|
} vehicle; |
|
|
|
static Location our_location; |
|
|
|
/** |
|
update the pitch servo. The aim is to drive the boards pitch to the |
|
requested pitch |
|
*/ |
|
static void update_pitch_servo(float pitch) |
|
{ |
|
channel_pitch.servo_out = g.pidPitch2Srv.get_pid_4500(degrees(ahrs.pitch) - pitch); |
|
channel_pitch.calc_pwm(); |
|
channel_pitch.output(); |
|
} |
|
|
|
/** |
|
update the yaw servo |
|
*/ |
|
static void update_yaw_servo(float yaw) |
|
{ |
|
channel_yaw.servo_out = g.pidYaw2Srv.get_pid_4500(degrees(ahrs.yaw) - yaw); |
|
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 |
|
if (g_gps->status() >= GPS::GPS_OK_FIX_2D) { |
|
our_location.lat = g_gps->latitude; |
|
our_location.lng = g_gps->longitude; |
|
our_location.alt = 0; // assume ground level for now |
|
} |
|
|
|
// calculate the bearing to the vehicle |
|
float bearing = get_bearing_cd(our_location, vehicle.location) * 0.01f; |
|
float distance = get_distance(our_location, vehicle.location); |
|
float pitch = degrees(atan2(vehicle.location.alt - our_location.alt, 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.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(); |
|
}
|
|
|