mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
AP_NavEKF2: Retrieve correct time-stamp for active compass
This commit is contained in:
parent
6698d4379d
commit
3099d94e78
@ -217,7 +217,7 @@ void NavEKF2_core::readMagData()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// store time of last measurement update
|
// store time of last measurement update
|
||||||
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec();
|
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex);
|
||||||
|
|
||||||
// estimate of time magnetometer measurement was taken, allowing for delays
|
// estimate of time magnetometer measurement was taken, allowing for delays
|
||||||
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
|
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
|
||||||
|
Loading…
Reference in New Issue
Block a user