Browse Source

AP_AHRS: getOptFlowSample returns latest correct flow data for use in calibration

gps-1.3.1
Randy Mackay 3 years ago
parent
commit
9126e14c9c
  1. 9
      libraries/AP_AHRS/AP_AHRS.cpp
  2. 3
      libraries/AP_AHRS/AP_AHRS.h

9
libraries/AP_AHRS/AP_AHRS.cpp

@ -2152,6 +2152,15 @@ void AP_AHRS::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &ra @@ -2152,6 +2152,15 @@ void AP_AHRS::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &ra
#endif
}
// retrieve latest corrected optical flow samples (used for calibration)
bool AP_AHRS::getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const
{
#if HAL_NAVEKF3_AVAILABLE
return EKF3.getOptFlowSample(timeStamp_ms, flowRate, bodyRate, losPred);
#endif
return false;
}
// write body frame odometry measurements to the EKF
void AP_AHRS::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
{

3
libraries/AP_AHRS/AP_AHRS.h

@ -240,6 +240,9 @@ public: @@ -240,6 +240,9 @@ public:
// write optical flow measurements to EKF
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset);
// retrieve latest corrected optical flow samples (used for calibration)
bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;
// write body odometry measurements to the EKF
void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset);

Loading…
Cancel
Save