Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: rename posxy_source_last and posxy_source_reset

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

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -535,7 +535,7 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
ResetHeight(); ResetHeight();
// initialise sources // initialise sources
pos_source_last = frontend->sources.getPosXYSource(); posxy_source_last = frontend->sources.getPosXYSource();
yaw_source_last = frontend->sources.getYawSource(); yaw_source_last = frontend->sources.getYawSource();
// define Earth rotation vector in the NED navigation frame // 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; uint8_t selected_airspeed;
// source reset handling // source reset handling
AP_NavEKF_Source::SourceXY pos_source_last; // position source on previous iteration (used to detect a changes) AP_NavEKF_Source::SourceXY posxy_source_last; // horizontal 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 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) 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 bool yaw_source_reset; // true when the yaw source has changed but the yaw has not yet been reset
}; };