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

Tracker: move vehicle pos estimate to separate function

Browse Source
This commit is contained in:
Randy Mackay 2014-10-01 22:24:06 +09:00
parent 1f91306a3b
commit a0e89281ef
2 changed files with 30 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
AntennaTracker/config.h
View File

@ -61,6 +61,9 @@
#ifndef TRACKING_TIMEOUT_MS #ifndef TRACKING_TIMEOUT_MS
# define TRACKING_TIMEOUT_MS 5000 // consider we've lost track of vehicle after 5 seconds with no position update. Used to update armed/disarmed status leds # define TRACKING_TIMEOUT_MS 5000 // consider we've lost track of vehicle after 5 seconds with no position update. Used to update armed/disarmed status leds
#endif #endif
#ifndef TRACKING_TIMEOUT_SEC
# define TRACKING_TIMEOUT_SEC 5.0f // consider we've lost track of vehicle after 5 seconds with no position update.
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Developer Items // Developer Items

33
AntennaTracker/tracking.pde
View File

@ -4,7 +4,9 @@
state of the vehicle we are tracking state of the vehicle we are tracking
*/ */
static struct { static struct {
bool location_valid; // true if we have a valid location for the vehicle
Location location; // lat, long in degrees * 10^7; alt in meters * 100 Location location; // lat, long in degrees * 10^7; alt in meters * 100
Location location_estimate; // lat, long in degrees * 10^7; alt in meters * 100
uint32_t last_update_us; // last position update in micxroseconds uint32_t last_update_us; // last position update in micxroseconds
uint32_t last_update_ms; // last position update in milliseconds uint32_t last_update_ms; // last position update in milliseconds
float heading; // last known direction vehicle is moving float heading; // last known direction vehicle is moving
@ -330,16 +332,35 @@ static void update_scan(void)
update_auto(); update_auto();
} }
/**
update_vehicle_position_estimate - updates estimate of vehicle positions
should be called at 50hz
*/
static void update_vehicle_pos_estimate()
{
// calculate time since last actual position update
float dt = (hal.scheduler->micros() - vehicle.last_update_us) * 1.0e-6f;
// if less than 5 seconds since last position update estimate the position
if (dt < TRACKING_TIMEOUT_SEC) {
// project the vehicle position to take account of lost radio packets
vehicle.location_estimate = vehicle.location;
location_update(vehicle.location_estimate, vehicle.heading, vehicle.ground_speed * dt);
// set valid_location flag
vehicle.location_valid = true;
} else {
// vehicle has been lost, set lost flag
vehicle.location_valid = false;
}
}
/** /**
main antenna tracking code, called at 50Hz main antenna tracking code, called at 50Hz
*/ */
static void update_tracking(void) static void update_tracking(void)
{ {
// project the vehicle position to take account of lost radio packets // update vehicle position estimate
Location vpos = vehicle.location; update_vehicle_pos_estimate();
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 // 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? // REVISIT: what if we lose lock during a mission and the antenna is moving?
@ -348,8 +369,8 @@ static void update_tracking(void)
} }
// 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_estimate) * 0.01f;
float distance = get_distance(current_loc, vehicle.location); float distance = get_distance(current_loc, vehicle.location_estimate);
float pitch = degrees(atan2f(nav_status.altitude_difference, distance)); float pitch = degrees(atan2f(nav_status.altitude_difference, distance));
// update nav_status for NAV_CONTROLLER_OUTPUT // update nav_status for NAV_CONTROLLER_OUTPUT