Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_L1_Control: adjust position for GPS lag

Browse Source 
this should help for rovers
This commit is contained in:
Andrew Tridgell 2013-08-12 13:26:40 +10:00
parent 57d2dd814d
commit 653fa5a191
1 changed files with 19 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -99,10 +99,15 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
// Get current position and velocity
_ahrs->get_projected_position(_current_loc);
Vector2f _groundspeed_vector = _ahrs->groundspeed_vector();
// update the position for lag. This helps especially for rovers
// where waypoints may be very close together
Vector2f lag_offset = _groundspeed_vector * _ahrs->get_position_lag();
location_offset(_current_loc, lag_offset.x, lag_offset.y);
// update _target_bearing_cd
_target_bearing_cd = get_bearing_cd(_current_loc, next_WP);
Vector2f _groundspeed_vector = _ahrs->groundspeed_vector();
//Calculate groundspeed
float groundSpeed = _groundspeed_vector.length();
@ -192,15 +197,22 @@ void AP_L1_Control::update_loiter(const struct Location &center_WP, float radius
float K_L1 = 4.0f * _L1_damping * _L1_damping;
//Get current position and velocity
_ahrs->get_projected_position(_current_loc);
_ahrs->get_position(_current_loc);
Vector2f _groundspeed_vector = _ahrs->groundspeed_vector();
// update the position for lag. This helps especially for rovers
// where waypoints may be very close together
Vector2f lag_offset = _groundspeed_vector * _ahrs->get_position_lag();
location_offset(_current_loc, lag_offset.x, lag_offset.y);
//Calculate groundspeed
float groundSpeed = max(_groundspeed_vector.length() , 1.0f);
// update _target_bearing_cd
_target_bearing_cd = get_bearing_cd(_current_loc, center_WP);
Vector2f _groundspeed_vector = _ahrs->groundspeed_vector();
//Calculate groundspeed
float groundSpeed = _maxf(_groundspeed_vector.length() , 1.0f);
// Calculate time varying control parameters
// Calculate the L1 length required for specified period