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

AP_NavEKF3: GPS change pos reset uses ResetPositonNE and ResetPositionD

Browse Source 
Using these existing functions removes some code duplicate
non-functional change
This commit is contained in:
Randy Mackay 2020-10-26 20:02:17 +09:00 committed by Andrew Tridgell
parent 73dea6d29c
commit 3b38aa6d7b
1 changed files with 3 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -484,52 +484,12 @@ void NavEKF3_core::SelectVelPosFusion()
// record the ID of the GPS that we are using for the reset
last_gps_idx = gpsDataDelayed.sensor_idx;
// Store the position before the reset so that we can record the reset delta
posResetNE.x = stateStruct.position.x;
posResetNE.y = stateStruct.position.y;
// Set the position states to the position from the new GPS
stateStruct.position.x = gpsDataDelayed.pos.x;
stateStruct.position.y = gpsDataDelayed.pos.y;
// Calculate the position offset due to the reset
posResetNE.x = stateStruct.position.x - posResetNE.x;
posResetNE.y = stateStruct.position.y - posResetNE.y;
// Add the offset to the output observer states
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].position.x += posResetNE.x;
storedOutput[i].position.y += posResetNE.y;
}
outputDataNew.position.x += posResetNE.x;
outputDataNew.position.y += posResetNE.y;
outputDataDelayed.position.x += posResetNE.x;
outputDataDelayed.position.y += posResetNE.y;
// store the time of the reset
lastPosReset_ms = imuSampleTime_ms;
// reset the position to the GPS position
ResetPositionNE(gpsDataDelayed.pos.x, gpsDataDelayed.pos.y);
// If we are also using GPS as the height reference, reset the height
if (activeHgtSource == HGT_SOURCE_GPS) {
// Store the position before the reset so that we can record the reset delta
posResetD = stateStruct.position.z;
// write to the state vector
stateStruct.position.z = -hgtMea;
// Calculate the position jump due to the reset
posResetD = stateStruct.position.z - posResetD;
// Add the offset to the output observer states
outputDataNew.position.z += posResetD;
vertCompFiltState.pos = outputDataNew.position.z;
outputDataDelayed.position.z += posResetD;
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].position.z += posResetD;
}
// store the time of the reset
lastPosResetD_ms = imuSampleTime_ms;
ResetPositionD(-hgtMea);
}
}