Rover: update current_loc in update_ahrs

this ensures the current_loc is updated before any of the flight mode code runs
This commit is contained in:
Randy Mackay 2018-05-23 14:34:47 +09:00
parent d7821635c0
commit cc53b9c39d
1 changed files with 3 additions and 1 deletions

View File

@ -159,6 +159,9 @@ void Rover::ahrs_update()
ahrs.update(); ahrs.update();
// update position
have_position = ahrs.get_position(current_loc);
// update home from EKF if necessary // update home from EKF if necessary
update_home_from_EKF(); update_home_from_EKF();
@ -303,7 +306,6 @@ void Rover::one_second_loop(void)
void Rover::update_GPS_10Hz(void) void Rover::update_GPS_10Hz(void)
{ {
have_position = ahrs.get_position(current_loc);
if (gps.last_message_time_ms() != last_gps_msg_ms) { if (gps.last_message_time_ms() != last_gps_msg_ms) {
last_gps_msg_ms = gps.last_message_time_ms(); last_gps_msg_ms = gps.last_message_time_ms();