aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: writeExtNavData accepts delay

Browse Source
This commit is contained in:
Randy Mackay 2020-05-09 07:46:18 +09:00
parent f26a2a47b4
commit c28fd27b02
4 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1324,14 +1324,15 @@ void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t tim
* posErr : 1-sigma spherical position error (m)
* angErr : 1-sigma spherical angle error (rad)
* timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
* delay_ms : average delay of external nav system measurements relative to inertial measurements
* resetTime_ms : system time of the last position reset request (mSec)
*
*/
void NavEKF3::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
void NavEKF3::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
{
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, resetTime_ms);
core[i].writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
}
}
}

3
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -289,10 +289,11 @@ public:
* posErr : 1-sigma spherical position error (m)
* angErr : 1-sigma spherical angle error (rad)
* timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
* delay_ms : average delay of external nav system measurements relative to inertial measurements
* resetTime_ms : system time of the last position reset request (mSec)
*
*/
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect

5
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -940,7 +940,7 @@ void NavEKF3_core::writeDefaultAirSpeed(float airspeed)
* External Navigation Measurements *
********************************************************/
void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
{
// 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
@ -967,8 +967,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
}
// calculate timestamp
const uint32_t extnav_delay_ms = 10;
timeStamp_ms = timeStamp_ms - extnav_delay_ms;
timeStamp_ms = timeStamp_ms - delay_ms;
// Correct for the average intersampling delay due to the filter update rate
timeStamp_ms -= localFilterTimeStep_ms/2;
// Prevent time delay exceeding age of oldest IMU data in the buffer

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -295,10 +295,11 @@ public:
* posErr : 1-sigma spherical position error (m)
* angErr : 1-sigma spherical angle error (rad)
* timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
* delay_ms : average delay of external nav system measurements relative to inertial measurements
* resetTime_ms : system time of the last position reset request (mSec)
*
*/
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect