mirror of https://github.com/ArduPilot/ardupilot
82 lines
2.3 KiB
Plaintext
82 lines
2.3 KiB
Plaintext
/// -*- 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();
|
|
}
|