Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: writeExtNavData stores pos to buffer first 

This is a non-functional change
This commit is contained in:
Randy Mackay 2020-06-15 11:29:40 +09:00
parent 578800dfbb
commit 331f2f5fe7
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -978,6 +978,9 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
timeStamp_ms = MAX(timeStamp_ms, imuDataDelayed.time_ms); timeStamp_ms = MAX(timeStamp_ms, imuDataDelayed.time_ms);
extNavDataNew.time_ms = timeStamp_ms; extNavDataNew.time_ms = timeStamp_ms;
// store position data to buffer
storedExtNav.push(extNavDataNew);
// protect against attitude or angle being NaN // protect against attitude or angle being NaN
if (!quat.is_nan() && !isnan(angErr)) { if (!quat.is_nan() && !isnan(angErr)) {
// extract yaw from the attitude // extract yaw from the attitude
@ -988,8 +991,6 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
const float yaw_accuracy_rad = MAX(angErr, radians(5.0f)); const float yaw_accuracy_rad = MAX(angErr, radians(5.0f));
writeEulerYawAngle(yaw_rad, yaw_accuracy_rad, timeStamp_ms, 2); writeEulerYawAngle(yaw_rad, yaw_accuracy_rad, timeStamp_ms, 2);
} }
storedExtNav.push(extNavDataNew);
} }
void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)