Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: writeBodyFrameOdom accepts delay

Browse Source
This commit is contained in:
Randy Mackay 2020-05-09 08:06:13 +09:00
parent ef02942459
commit b2ad1caf75
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -388,7 +388,7 @@ void AP_AHRS_NavEKF::update_SITL(void)
const Vector3f earth_vel(fdm.speedN,fdm.speedE,fdm.speedD); const Vector3f earth_vel(fdm.speedN,fdm.speedE,fdm.speedD);
const Vector3f delPos = Tbn.transposed() * (earth_vel * delTime); const Vector3f delPos = Tbn.transposed() * (earth_vel * delTime);
// write to EKF // 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 #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 // 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 #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 #endif
} }

2
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -178,7 +178,7 @@ public:
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset); 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 // 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. // 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); void writeDefaultAirSpeed(float airspeed);