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

AP_NavEKF : temporary mods to test use of flow sensor internal gyro data

Browse Source
This commit is contained in:
priseborough 2014-08-16 18:41:54 +10:00 committed by Andrew Tridgell
parent 12b012a00e
commit 81b09c9361
2 changed files with 13 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3525,8 +3525,8 @@ void NavEKF::getFlowDebug(float &scaleFactor, float &obsX, float &obsY, float &i
scaleFactor = flowStates[0];
obsX = flowRadXY[0];
obsY = flowRadXY[1];
innovX = innovOptFlow[0];
innovY = innovOptFlow[1];
innovX = omegaAcrossFlowTime.x;//innovOptFlow[0];
innovY = omegaAcrossFlowTime.y;//innovOptFlow[1];
gndPos = flowStates[1];
quality = flowQuality;
}
@ -3902,7 +3902,7 @@ void NavEKF::readAirSpdData()
// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, 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
// A modified PX4flow firmware has been used which effectivly sets the range used to calculate the velocity internally within the px4flow sensor to unity
@ -3916,6 +3916,11 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, f
RecallStates(statesAtFlowTime, flowMeaTime_ms - _msecFLowDelay - flowTimeDeltaAvg_ms/2);
// recall angular rates averaged across flow observation period allowing for average 5 msec intersample delay
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
flowGyroBias.x = 0.999f * flowGyroBias.x + 0.001f * (rawGyroRates.x - omegaAcrossFlowTime.x);
flowGyroBias.y = 0.999f * flowGyroBias.y + 0.001f * (rawGyroRates.y - omegaAcrossFlowTime.y);
omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x;
omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y;
// calculate rotation matrices at mid sample time for flow observations
Quaternion q(statesAtFlowTime.quat[0],statesAtFlowTime.quat[1],statesAtFlowTime.quat[2],statesAtFlowTime.quat[3]);
q.rotation_matrix(Tbn_flow);
@ -4184,6 +4189,8 @@ void NavEKF::ZeroVariables()
fuseRngData = false;
inhibitGndState = true;
prevFlowFusionTime_ms = imuSampleTime_ms; // time the last flow measurement was fused
flowGyroBias.x = 0;
flowGyroBias.y = 0;
}
// return true if we should use the airspeed sensor

4
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -157,9 +157,10 @@ public:
// write the raw optical flow measurements
// rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality
// rawFlowRates is the flow rate in radians per second. A positive ground relative velocity along the X axis produces positive raw X value, and similarly for the Y axis
// rawGyroRates are the sensor phsyical rotation rates measured by the internal gyro
// rawSonarRange is the range in metres measured by the px4flow sensor
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, float &rawSonarRange, uint32_t &msecFlowMeas);
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, float &rawSonarRange, uint32_t &msecFlowMeas);
// return data for debugging optical flow fusion
void getFlowDebug(float &scaleFactor, float &obsX, float &obsY, float &innovX, float &innovY, float &gndPos, uint8_t &quality) const;
@ -579,6 +580,7 @@ private:
float auxFlowTestRatio[2]; // sum of squares of optical flow innovations divided by fail threshold used by aux filter
float R_LOS; // variance of optical flow rate measurements (rad/sec)^2
float auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
Vector2f flowGyroBias; // bias error of optical flow sensor gyro output
// states held by optical flow fusion across time steps
// optical flow X,Y motion compensated rate measurements are fused across two time steps