Browse Source

AP_NavEKF: avoid an extra quaternion copy

mission-4.1.18
Andrew Tridgell 10 years ago
parent
commit
16affe51be
  1. 4
      libraries/AP_NavEKF/AP_NavEKF.cpp

4
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -4071,10 +4071,10 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V @@ -4071,10 +4071,10 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
// recall vehicle states at mid sample time for flow observations allowing for delays
RecallStates(statesAtFlowTime, imuSampleTime_ms - _msecFLowDelay - flowTimeDeltaAvg_ms/2);
// 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);
statesAtFlowTime.quat.rotation_matrix(Tbn_flow);
Tnb_flow = Tbn_flow.transposed();
// 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

Loading…
Cancel
Save