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

AP_NavEKF3: rename posxy_source_last and posxy_source_reset

This commit is contained in:
Randy Mackay 2020-11-18 15:21:45 +09:00
parent f88364d653
commit 5eb3875ebb
3 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -448,10 +448,10 @@ void NavEKF3_core::SelectVelPosFusion()
}
// detect position source changes. Trigger position reset if position source is valid
AP_NavEKF_Source::SourceXY pos_source = frontend->sources.getPosXYSource();
if (pos_source != pos_source_last) {
pos_source_reset = (pos_source != AP_NavEKF_Source::SourceXY::NONE);
pos_source_last = pos_source;
const AP_NavEKF_Source::SourceXY posxy_source = frontend->sources.getPosXYSource();
if (posxy_source != posxy_source_last) {
posxy_source_reset = (posxy_source != AP_NavEKF_Source::SourceXY::NONE);
posxy_source_last = posxy_source;
}
// initialise all possible data we may fuse
@ -459,7 +459,7 @@ void NavEKF3_core::SelectVelPosFusion()
fuseVelData = false;
// Determine if we need to fuse position and velocity data on this time step
if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS)) {
if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::GPS)) {
// Don't fuse velocity data if GPS doesn't support it
fuseVelData = frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS);
@ -501,9 +501,9 @@ void NavEKF3_core::SelectVelPosFusion()
selectHeightForFusion();
// if we are using GPS, check for a change in receiver and reset position and height
if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) && (gpsDataDelayed.sensor_idx != last_gps_idx || pos_source_reset)) {
if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) && (gpsDataDelayed.sensor_idx != last_gps_idx || posxy_source_reset)) {
// mark a source reset as consumed
pos_source_reset = false;
posxy_source_reset = false;
// record the ID of the GPS that we are using for the reset
last_gps_idx = gpsDataDelayed.sensor_idx;
@ -518,9 +518,9 @@ void NavEKF3_core::SelectVelPosFusion()
}
// check for external nav position reset
if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV) && (extNavDataDelayed.posReset || pos_source_reset)) {
if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV) && (extNavDataDelayed.posReset || posxy_source_reset)) {
// mark a source reset as consumed
pos_source_reset = false;
posxy_source_reset = false;
ResetPositionNE(extNavDataDelayed.pos.x, extNavDataDelayed.pos.y);
if (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV) {
ResetPositionD(-hgtMea);

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -535,7 +535,7 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
ResetHeight();
// initialise sources
pos_source_last = frontend->sources.getPosXYSource();
posxy_source_last = frontend->sources.getPosXYSource();
yaw_source_last = frontend->sources.getYawSource();
// define Earth rotation vector in the NED navigation frame

4
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1491,8 +1491,8 @@ private:
uint8_t selected_airspeed;
// source reset handling
AP_NavEKF_Source::SourceXY pos_source_last; // position source on previous iteration (used to detect a changes)
bool pos_source_reset; // true when the position source has changed but the position has not yet been reset
AP_NavEKF_Source::SourceXY posxy_source_last; // horizontal position source on previous iteration (used to detect a changes)
bool posxy_source_reset; // true when the horizontal position source has changed but the position has not yet been reset
AP_NavEKF_Source::SourceYaw yaw_source_last; // yaw source on previous iteration (used to detect a change)
bool yaw_source_reset; // true when the yaw source has changed but the yaw has not yet been reset
};