// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas)
{
// The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
// The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
// A positive X rate is produced by a positive sensor rotation about the X axis
// A positive Y rate is produced by a positive sensor rotation about the Y axis
// 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 = imuSampleTime_ms;
// calculate bias errors on flow sensor gyro rates, but protect against spikes in data
// reset the accumulated body delta angle and time
// don't do the calculation if not enough time lapsed for a reliable body rate measurement
if (delTimeOF > 0.01f) {
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
delAngBodyOF.zero();
delTimeOF = 0.0f;
}
// check for takeoff if relying on optical flow and zero measurements until takeoff detected
// if we haven't taken off - constrain position and velocity states
if (frontend->_fusionModeGPS == 3) {
detectOptFlowTakeoff();
}
// calculate rotation matrices at mid sample time for flow observations
stateStruct.quat.rotation_matrix(Tbn_flow);
Tnb_flow = Tbn_flow.transposed();
// don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
// correct flow sensor rates for bias
omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x;
omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y;
// 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
ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
// write flow rate measurements corrected for body rates
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + omegaAcrossFlowTime.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + omegaAcrossFlowTime.y;
// record time last observation was received so we can detect loss of data elsewhere
flowValidMeaTime_ms = imuSampleTime_ms;
// estimate sample time of the measurement
ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
ofDataNew.time_ms = roundToNearest(ofDataNew.time_ms, frontend->fusionTimeStep_ms);
// Prevent time delay exceeding age of oldest IMU data in the buffer
ofDataNew.time_ms = max(ofDataNew.time_ms,imuDataDelayed.time_ms);
// Save data to buffer
StoreOF();
// Check for data at the fusion time horizon
newDataFlow = RecallOF();
}
}
// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas)
{
// The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
// The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
// A positive X rate is produced by a positive sensor rotation about the X axis
// A positive Y rate is produced by a positive sensor rotation about the Y axis
// 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 = imuSampleTime_ms;
// calculate bias errors on flow sensor gyro rates, but protect against spikes in data
// reset the accumulated body delta angle and time
// don't do the calculation if not enough time lapsed for a reliable body rate measurement
if (delTimeOF > 0.01f) {
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
delAngBodyOF.zero();
delTimeOF = 0.0f;
}
// by definition if this function is called, then flow measurements have been provided so we
// need to run the optical flow takeoff detection
detectOptFlowTakeoff();
// calculate rotation matrices at mid sample time for flow observations
stateStruct.quat.rotation_matrix(Tbn_flow);
// don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
// correct flow sensor rates for bias
omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x;
omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y;
// 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
ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
// write flow rate measurements corrected for body rates
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + omegaAcrossFlowTime.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + omegaAcrossFlowTime.y;
// record time last observation was received so we can detect loss of data elsewhere
flowValidMeaTime_ms = imuSampleTime_ms;
// estimate sample time of the measurement
ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
// Correct for the average intersampling delay due to the filter updaterate
ofDataNew.time_ms -= localFilterTimeStep_ms/2;
// Prevent time delay exceeding age of oldest IMU data in the buffer
ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms);
// Save data to buffer
storedOF.push(ofDataNew);
// Check for data at the fusion time horizon
flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms);
}
}
