mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Plane: Update GPS before updating current_loc
Fixes 20ms of extra induced lag on the DCM position estimate
This commit is contained in:
parent
ce53ae63ae
commit
35bd143532
@ -359,12 +359,12 @@ void Plane::airspeed_ratio_update(void)
|
|||||||
*/
|
*/
|
||||||
void Plane::update_GPS_50Hz(void)
|
void Plane::update_GPS_50Hz(void)
|
||||||
{
|
{
|
||||||
|
gps.update();
|
||||||
|
|
||||||
// get position from AHRS
|
// get position from AHRS
|
||||||
have_position = ahrs.get_position(current_loc);
|
have_position = ahrs.get_position(current_loc);
|
||||||
ahrs.get_relative_position_D_home(relative_altitude);
|
ahrs.get_relative_position_D_home(relative_altitude);
|
||||||
relative_altitude *= -1.0f;
|
relative_altitude *= -1.0f;
|
||||||
|
|
||||||
gps.update();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
Loading…
Reference in New Issue
Block a user