Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: writeBodyFrameOdom accepts delay

This commit is contained in:
Randy Mackay 2020-05-09 08:05:40 +09:00
parent 69560ec147
commit ef02942459
4 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1355,13 +1355,14 @@ void NavEKF3::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, fl
* delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at timeStamp_ms (rad) * delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at timeStamp_ms (rad)
* delTime is the time interval for the measurement of delPos and delAng (sec) * delTime is the time interval for the measurement of delPos and delAng (sec)
* timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec) * timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec)
* delay_ms is the average delay of external nav system measurements relative to inertial measurements
* posOffset is the XYZ body frame position of the camera focal point (m) * posOffset is the XYZ body frame position of the camera focal point (m)
*/ */
void NavEKF3::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset) void NavEKF3::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
{ {
if (core) { if (core) {
for (uint8_t i=0; i<num_cores; i++) { for (uint8_t i=0; i<num_cores; i++) {
core[i].writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset); core[i].writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, delay_ms, posOffset);
} }
} }
} }

3
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -220,9 +220,10 @@ public:
* delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at timeStamp_ms (rad) * delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at timeStamp_ms (rad)
* delTime is the time interval for the measurement of delPos and delAng (sec) * delTime is the time interval for the measurement of delPos and delAng (sec)
* timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec) * timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec)
* delay_ms is the average delay of external nav system measurements relative to inertial measurements
* posOffset is the XYZ body frame position of the camera focal point (m) * posOffset is the XYZ body frame position of the camera focal point (m)
*/ */
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);
/* /*
* Write odometry data from a wheel encoder. The axis of rotation is assumed to be parallel to the vehicle body axis * Write odometry data from a wheel encoder. The axis of rotation is assumed to be parallel to the vehicle body axis

5
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -118,7 +118,7 @@ void NavEKF3_core::readRangeFinder(void)
} }
} }
void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset) void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
{ {
// limit update rate to maximum allowed by sensor buffers and fusion process // limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised // don't try to write to buffer until the filter has been initialised
@ -126,6 +126,9 @@ void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, con
return; return;
} }
// subtract delay from timestamp
timeStamp_ms -= delay_ms;
bodyOdmDataNew.body_offset = &posOffset; bodyOdmDataNew.body_offset = &posOffset;
bodyOdmDataNew.vel = delPos * (1.0f/delTime); bodyOdmDataNew.vel = delPos * (1.0f/delTime);
bodyOdmDataNew.time_ms = timeStamp_ms; bodyOdmDataNew.time_ms = timeStamp_ms;

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -231,9 +231,10 @@ public:
* delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at time_ms (rad) * delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at time_ms (rad)
* delTime is the time interval for the measurement of delPos and delAng (sec) * delTime is the time interval for the measurement of delPos and delAng (sec)
* timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec) * timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec)
* delay_ms is the average delay of external nav system measurements relative to inertial measurements
* posOffset is the XYZ body frame position of the camera focal point (m) * posOffset is the XYZ body frame position of the camera focal point (m)
*/ */
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);
/* /*
* Write odometry data from a wheel encoder. The axis of rotation is assumed to be parallel to the vehicle body axis * Write odometry data from a wheel encoder. The axis of rotation is assumed to be parallel to the vehicle body axis