Plane: Update GPS before updating current_loc

Fixes 20ms of extra induced lag on the DCM position estimate
This commit is contained in:
Michael du Breuil 2019-04-05 00:52:33 -07:00 committed by Tom Pittenger
parent ce53ae63ae
commit 35bd143532

View File

@ -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();
} }
/* /*