Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: ext nav vel corrected for sensor position when recalled from buffer

Browse Source
This commit is contained in:
Randy Mackay 2020-06-01 17:59:37 +09:00
parent 79901ffc1b
commit d37eec5fd8
2 changed files with 10 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -47,10 +47,8 @@ void NavEKF3_core::ResetVelocity(void)
lastVelPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms;
} else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && posResetSource == DEFAULT) || velResetSource == EXTNAV) { } else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && posResetSource == DEFAULT) || velResetSource == EXTNAV) {
// use external nav data as the 2nd preference // use external nav data as the 2nd preference
// correct for sensor position // already corrected for sensor position
ext_nav_vel_elements extNavVelCorrected = extNavVelDelayed; stateStruct.velocity = extNavVelDelayed.vel;
CorrectExtNavVelForSensorOffset(extNavVelCorrected.vel);
stateStruct.velocity = extNavVelCorrected.vel;
P[5][5] = P[4][4] = P[3][3] = sq(extNavVelDelayed.err); P[5][5] = P[4][4] = P[3][3] = sq(extNavVelDelayed.err);
velTimeout = false; velTimeout = false;
lastVelPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms;
@ -405,6 +403,9 @@ void NavEKF3_core::SelectVelPosFusion()
// Check for data at the fusion time horizon // Check for data at the fusion time horizon
extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms); extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
extNavVelToFuse = storedExtNavVel.recall(extNavVelDelayed, imuDataDelayed.time_ms); extNavVelToFuse = storedExtNavVel.recall(extNavVelDelayed, imuDataDelayed.time_ms);
if (extNavVelToFuse) {
CorrectExtNavVelForSensorOffset(extNavVelDelayed.vel);
}
// Read GPS data from the sensor // Read GPS data from the sensor
readGpsData(); readGpsData();
@ -454,15 +455,12 @@ void NavEKF3_core::SelectVelPosFusion()
} }
// fuse external navigation velocity data if available // fuse external navigation velocity data if available
// extNavVelDelayed is already corrected for sensor position
if (extNavVelToFuse && (frontend->_fusionModeGPS == 3)) { if (extNavVelToFuse && (frontend->_fusionModeGPS == 3)) {
fuseVelData = true; fuseVelData = true;
velPosObs[0] = extNavVelDelayed.vel.x;
// correct for external navigation sensor position velPosObs[1] = extNavVelDelayed.vel.y;
Vector3f vel_corrected = extNavVelDelayed.vel; velPosObs[2] = extNavVelDelayed.vel.z;
CorrectExtNavVelForSensorOffset(vel_corrected);
velPosObs[0] = vel_corrected.x;
velPosObs[1] = vel_corrected.y;
velPosObs[2] = vel_corrected.z;
} }
// we have GPS data to fuse and a request to align the yaw using the GPS course // we have GPS data to fuse and a request to align the yaw using the GPS course

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1357,7 +1357,7 @@ private:
bool extNavUsedForPos; // true when the external nav data is being used as a position reference. bool extNavUsedForPos; // true when the external nav data is being used as a position reference.
obs_ring_buffer_t<ext_nav_vel_elements> storedExtNavVel; // external navigation velocity data buffer obs_ring_buffer_t<ext_nav_vel_elements> storedExtNavVel; // external navigation velocity data buffer
ext_nav_vel_elements extNavVelNew; // external navigation velocity data at the current time horizon ext_nav_vel_elements extNavVelNew; // external navigation velocity data at the current time horizon
ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon. Already corrected for sensor position
uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec) uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec)
bool extNavVelToFuse; // true when there is new external navigation velocity to fuse bool extNavVelToFuse; // true when there is new external navigation velocity to fuse
bool useExtNavVel; // true if external nav velocity should be used bool useExtNavVel; // true if external nav velocity should be used