From b2ad1caf75deec5f34c3e135f60fcd31050c34ba Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 9 May 2020 08:06:13 +0900 Subject: [PATCH] AP_AHRS: writeBodyFrameOdom accepts delay --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 6 +++--- libraries/AP_AHRS/AP_AHRS_NavEKF.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 1d191cb617..a1e109501f 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -388,7 +388,7 @@ void AP_AHRS_NavEKF::update_SITL(void) const Vector3f earth_vel(fdm.speedN,fdm.speedE,fdm.speedD); const Vector3f delPos = Tbn.transposed() * (earth_vel * delTime); // write to EKF - EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset); + EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, 0, posOffset); } } #endif // HAL_NAVEKF3_AVAILABLE @@ -1445,10 +1445,10 @@ void AP_AHRS_NavEKF::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vecto } // write body frame odometry measurements to the EKF -void AP_AHRS_NavEKF::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset) +void AP_AHRS_NavEKF::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset) { #if HAL_NAVEKF3_AVAILABLE - EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset); + EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, delay_ms, posOffset); #endif } diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 7625afa863..b034d35b58 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -178,7 +178,7 @@ public: void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset); // write body odometry measurements to the EKF - void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset); + void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset); // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available. void writeDefaultAirSpeed(float airspeed);