Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NAvEKF : update comments

Browse Source
This commit is contained in:
priseborough 2014-08-16 21:41:14 +10:00 committed by Andrew Tridgell
parent 81b09c9361
commit 3fbedc9f98
1 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3904,21 +3904,23 @@ void NavEKF::readAirSpdData()
// this needs to be called externally. // this needs to be called externally.
void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, float &rawSonarRange, uint32_t &msecFlowMeas) void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, float &rawSonarRange, uint32_t &msecFlowMeas)
{ {
// The raw measurements need to be optical flow rates in radians/second, however they are currently output by the sensor as pixels over an internal sampling period // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
// A modified PX4flow firmware has been used which effectivly sets the range used to calculate the velocity internally within the px4flow sensor to unity // The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
// so that the velocity outputs are in radians/sec // A positive X rate is produced by a positive velocity over ground in the X direction
// This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a negative sensor rotation about that axis, // A positive Y rate is produced by a positive velocity over ground in the Y direction
// hence the need to swap axes and signs. // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
// 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
flowMeaTime_ms = msecFlowMeas; flowMeaTime_ms = msecFlowMeas;
if (rawFlowQuality > 150){ if (rawFlowQuality > 150){
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, flowMeaTime_ms - _msecFLowDelay - flowTimeDeltaAvg_ms/2);
// recall angular rates averaged across flow observation period allowing for average 5 msec intersample delay // 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, flowMeaTime_ms - flowTimeDeltaAvg_ms - _msecFLowDelay, flowMeaTime_ms - _msecFLowDelay);
// Try using flow sensor internal rates for motion compensation. These need to be de-biased // 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);
// correct flow sensor rates for bias
omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x; omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x;
omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y; omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y;
// calculate rotation matrices at mid sample time for flow observations // calculate rotation matrices at mid sample time for flow observations
@ -3926,6 +3928,7 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
q.rotation_matrix(Tbn_flow); q.rotation_matrix(Tbn_flow);
Tnb_flow = Tbn_flow.transposed(); Tnb_flow = Tbn_flow.transposed();
// write uncorrected flow rate measurements that will be used by the focal length scale factor estimator // write uncorrected flow rate measurements that will be used by the focal length scale factor estimator
// note correction for different axis and sign conventions used by the px4flow sensor
flowRadXY[0] = + rawFlowRates.y; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec) flowRadXY[0] = + rawFlowRates.y; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
flowRadXY[1] = - rawFlowRates.x; // raw (non motion compensated) optical flow angular rate about the Y axis (rad/sec) flowRadXY[1] = - rawFlowRates.x; // raw (non motion compensated) optical flow angular rate about the Y axis (rad/sec)
// write flow rate measurements corrected for focal length scale factor errors and body rates // write flow rate measurements corrected for focal length scale factor errors and body rates