Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Fix incorrect use of flow sensor time-stamp

Browse Source 
flow sensor time stamp is now issued by the flow sensor and is on a different time-base
This commit is contained in:
priseborough 2014-11-01 10:49:34 +11:00 committed by Andrew Tridgell
parent 92e9336fe1
commit 573633daf7
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4007,9 +4007,9 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
// negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate // negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate
flowQuality = rawFlowQuality; flowQuality = rawFlowQuality;
// recall vehicle states at mid sample time for flow observations allowing for delays // recall vehicle states at mid sample time for flow observations allowing for delays
RecallStates(statesAtFlowTime, flowMeaTime_ms - _msecFLowDelay - flowTimeDeltaAvg_ms/2); RecallStates(statesAtFlowTime, imuSampleTime_ms - _msecFLowDelay - flowTimeDeltaAvg_ms/2);
// recall angular rates averaged across flow observation period allowing for processing, transmission and intersample delays // recall angular rates averaged across flow observation period allowing for processing, transmission and intersample delays
RecallOmega(omegaAcrossFlowTime, flowMeaTime_ms - flowTimeDeltaAvg_ms - _msecFLowDelay, flowMeaTime_ms - _msecFLowDelay); RecallOmega(omegaAcrossFlowTime, imuSampleTime_ms - flowTimeDeltaAvg_ms - _msecFLowDelay, flowMeaTime_ms - _msecFLowDelay);
// calculate bias errors on flow sensor gyro rates // calculate bias errors on flow sensor gyro rates
flowGyroBias.x = 0.999f * flowGyroBias.x + 0.001f * (rawGyroRates.x - omegaAcrossFlowTime.x); flowGyroBias.x = 0.999f * flowGyroBias.x + 0.001f * (rawGyroRates.x - omegaAcrossFlowTime.x);
flowGyroBias.y = 0.999f * flowGyroBias.y + 0.001f * (rawGyroRates.y - omegaAcrossFlowTime.y); flowGyroBias.y = 0.999f * flowGyroBias.y + 0.001f * (rawGyroRates.y - omegaAcrossFlowTime.y);
@ -4031,7 +4031,7 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
// set flag that will trigger observations // set flag that will trigger observations
newDataFlow = true; newDataFlow = true;
holdVelocity = false; holdVelocity = false;
flowMeaTime_ms = msecFlowMeas; flowMeaTime_ms = imuSampleTime_ms;
} else { } else {
newDataFlow = false; newDataFlow = false;
} }
@ -4040,7 +4040,7 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
statesAtRngTime = statesAtFlowTime; statesAtRngTime = statesAtFlowTime;
rngMea = rawSonarRange; rngMea = rawSonarRange;
newDataRng = true; newDataRng = true;
rngMeaTime_ms = msecFlowMeas; rngMeaTime_ms = imuSampleTime_ms;
} else { } else {
newDataRng = false; newDataRng = false;
} }