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

AntennaTracker: finish adding GPS support, including using relative or absolute altitudes

This commit is contained in:
Mike McCauley 2014-03-05 15:47:47 +10:00 committed by Andrew Tridgell
parent b1e00c695e
commit 05646904de
3 changed files with 17 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Tools/AntennaTracker/AntennaTracker.pde
View File

@ -191,7 +191,6 @@ static GCS_MAVLINK gcs3;
// Location structure defined in AP_Common
////////////////////////////////////////////////////////////////////////////////
static struct Location current_loc;
static struct Location home_loc;
/*
scheduler table - all regular tasks apart from the fast_loop()

5
Tools/AntennaTracker/system.pde
View File

@ -72,7 +72,8 @@ static void init_tracker()
channel_yaw.calc_pwm();
channel_pitch.calc_pwm();
home_loc = get_home_eeprom(); // GPS may update this later
current_loc = get_home_eeprom(); // GPS may update this later
arm_servos();
gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track."));
@ -187,7 +188,7 @@ static void set_home(struct Location temp)
if (g.compass_enabled)
compass.set_initial_location(temp.lat, temp.lng);
set_home_eeprom(temp);
home_loc = temp;
current_loc = temp;
}
static void arm_servos()

23
Tools/AntennaTracker/tracking.pde
View File

@ -4,10 +4,11 @@
state of the vehicle we are tracking
*/
static struct {
Location location; // lat, long in degrees * 10^7; alt in meters * 100
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
float heading; // degrees
float ground_speed; // m/s
} vehicle;
/**
@ -163,18 +164,21 @@ static void update_tracking(void)
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 = 0; // assume ground level for now REVISIT: WHY?
}
else {
current_loc = home_loc; // dont know any better
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);
float pitch = degrees(atan2((vehicle.location.alt - current_loc.alt)/100, distance));
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);
@ -193,7 +197,8 @@ 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.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();