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

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

Browse Source
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 // Location structure defined in AP_Common
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
static struct Location current_loc; static struct Location current_loc;
static struct Location home_loc;
/* /*
scheduler table - all regular tasks apart from the fast_loop() 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_yaw.calc_pwm();
channel_pitch.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(); arm_servos();
gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track.")); 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) if (g.compass_enabled)
compass.set_initial_location(temp.lat, temp.lng); compass.set_initial_location(temp.lat, temp.lng);
set_home_eeprom(temp); set_home_eeprom(temp);
home_loc = temp; current_loc = temp;
} }
static void arm_servos() static void arm_servos()

17
Tools/AntennaTracker/tracking.pde
View File

@ -5,6 +5,7 @@
*/ */
static struct { 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; uint32_t last_update_us;
float heading; // degrees float heading; // degrees
float ground_speed; // m/s float ground_speed; // m/s
@ -163,18 +164,21 @@ static void update_tracking(void)
location_update(vpos, vehicle.heading, vehicle.ground_speed * dt); location_update(vpos, vehicle.heading, vehicle.ground_speed * dt);
// update our position if we have at least a 2D fix // 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) { if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
current_loc.lat = g_gps->latitude; current_loc.lat = g_gps->latitude;
current_loc.lng = g_gps->longitude; current_loc.lng = g_gps->longitude;
current_loc.alt = 0; // assume ground level for now REVISIT: WHY? current_loc.alt = g_gps->altitude_cm;
} current_loc.options = 0; // Absolute altitude
else {
current_loc = home_loc; // dont know any better
} }
// calculate the bearing to the vehicle // calculate the bearing to the vehicle
float bearing = get_bearing_cd(current_loc, vehicle.location) * 0.01f; float bearing = get_bearing_cd(current_loc, vehicle.location) * 0.01f;
float distance = get_distance(current_loc, vehicle.location); 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 the servos
update_pitch_servo(pitch); update_pitch_servo(pitch);
update_yaw_servo(bearing); 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.lat = msg.lat;
vehicle.location.lng = msg.lon; 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.heading = msg.hdg * 0.01f;
vehicle.ground_speed = pythagorous2(msg.vx, msg.vy) * 0.01f; vehicle.ground_speed = pythagorous2(msg.vx, msg.vy) * 0.01f;
vehicle.last_update_us = hal.scheduler->micros(); vehicle.last_update_us = hal.scheduler->micros();