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

AP_NavEKF3: pos vel resets default to user defined source

This commit is contained in:
Randy Mackay 2024-11-20 20:26:55 +09:00 committed by Andrew Tridgell
parent 8b8a83f7c0
commit dce0370f76
1 changed files with 40 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -13,6 +13,26 @@
// Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
{
// if reset source is not specified then use user defined velocity source
if (velResetSource == resetDataSource::DEFAULT) {
switch (frontend->sources.getVelXYSource()) {
case AP_NavEKF_Source::SourceXY::GPS:
velResetSource = resetDataSource::GPS;
break;
case AP_NavEKF_Source::SourceXY::BEACON:
velResetSource = resetDataSource::RNGBCN;
break;
case AP_NavEKF_Source::SourceXY::EXTNAV:
velResetSource = resetDataSource::EXTNAV;
break;
case AP_NavEKF_Source::SourceXY::NONE:
case AP_NavEKF_Source::SourceXY::OPTFLOW:
case AP_NavEKF_Source::SourceXY::WHEEL_ENCODER:
// unhandled sources so stick with the default
break;
}
}
// Store the velocity before the reset so that we can record the reset delta
velResetNE.x = stateStruct.velocity.x;
velResetNE.y = stateStruct.velocity.y;
@ -73,6 +93,26 @@ void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
// resets position states to last GPS measurement or to zero if in constant position mode
void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
{
// if reset source is not specified thenn use the user defined position source
if (posResetSource == resetDataSource::DEFAULT) {
switch (frontend->sources.getPosXYSource()) {
case AP_NavEKF_Source::SourceXY::GPS:
posResetSource = resetDataSource::GPS;
break;
case AP_NavEKF_Source::SourceXY::BEACON:
posResetSource = resetDataSource::RNGBCN;
break;
case AP_NavEKF_Source::SourceXY::EXTNAV:
posResetSource = resetDataSource::EXTNAV;
break;
case AP_NavEKF_Source::SourceXY::NONE:
case AP_NavEKF_Source::SourceXY::OPTFLOW:
case AP_NavEKF_Source::SourceXY::WHEEL_ENCODER:
// invalid sources so stick with the default
break;
}
}
// Store the position before the reset so that we can record the reset delta
posResetNE.x = stateStruct.position.x;
posResetNE.y = stateStruct.position.y;