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

AP_NavEKF3: CorrectGPSForAntennaOffset made const

Browse Source 
also reduce scope of call to slightly reduce CPU load
This commit is contained in:
Randy Mackay 2020-04-16 15:25:16 +09:00 committed by Andrew Tridgell
parent 85704e4989
commit 7470bc9994
2 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -28,9 +28,6 @@ void NavEKF3_core::ResetVelocity(void)
zeroRows(P,4,5); zeroRows(P,4,5);
zeroCols(P,4,5); zeroCols(P,4,5);
gps_elements gps_corrected = gpsDataNew;
CorrectGPSForAntennaOffset(gps_corrected);
if (PV_AidingMode != AID_ABSOLUTE) { if (PV_AidingMode != AID_ABSOLUTE) {
stateStruct.velocity.zero(); stateStruct.velocity.zero();
// set the variances using the measurement noise parameter // set the variances using the measurement noise parameter
@ -38,6 +35,9 @@ void NavEKF3_core::ResetVelocity(void)
} else { } else {
// reset horizontal velocity states to the GPS velocity if available // reset horizontal velocity states to the GPS velocity if available
if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && velResetSource == DEFAULT) || velResetSource == GPS) { if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && velResetSource == DEFAULT) || velResetSource == GPS) {
// correct for antenna position
gps_elements gps_corrected = gpsDataNew;
CorrectGPSForAntennaOffset(gps_corrected);
stateStruct.velocity.x = gps_corrected.vel.x; stateStruct.velocity.x = gps_corrected.vel.x;
stateStruct.velocity.y = gps_corrected.vel.y; stateStruct.velocity.y = gps_corrected.vel.y;
// set the variances using the reported GPS speed accuracy // set the variances using the reported GPS speed accuracy
@ -94,11 +94,11 @@ void NavEKF3_core::ResetPosition(void)
// set the variances using the position measurement noise parameter // set the variances using the position measurement noise parameter
P[7][7] = P[8][8] = sq(frontend->_gpsHorizPosNoise); P[7][7] = P[8][8] = sq(frontend->_gpsHorizPosNoise);
} else { } else {
gps_elements gps_corrected = gpsDataNew;
CorrectGPSForAntennaOffset(gps_corrected);
// Use GPS data as first preference if fresh data is available // Use GPS data as first preference if fresh data is available
if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && posResetSource == DEFAULT) || posResetSource == GPS) { if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && posResetSource == DEFAULT) || posResetSource == GPS) {
// correct for antenna position
gps_elements gps_corrected = gpsDataNew;
CorrectGPSForAntennaOffset(gps_corrected);
// record the ID of the GPS for the data we are using for the reset // record the ID of the GPS for the data we are using for the reset
last_gps_idx = gps_corrected.sensor_idx; last_gps_idx = gps_corrected.sensor_idx;
// write to state vector and compensate for offset between last GPS measurement and the EKF time horizon // write to state vector and compensate for offset between last GPS measurement and the EKF time horizon
@ -249,7 +249,7 @@ bool NavEKF3_core::resetHeightDatum(void)
/* /*
correct GPS data for position offset of antenna phase centre relative to the IMU correct GPS data for position offset of antenna phase centre relative to the IMU
*/ */
void NavEKF3_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) void NavEKF3_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
{ {
const Vector3f &posOffsetBody = AP::gps().get_antenna_offset(gps_data.sensor_idx) - accelPosOffset; const Vector3f &posOffsetBody = AP::gps().get_antenna_offset(gps_data.sensor_idx) - accelPosOffset;
if (posOffsetBody.is_zero()) { if (posOffsetBody.is_zero()) {

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -865,7 +865,7 @@ private:
void updateStateIndexLim(void); void updateStateIndexLim(void);
// correct GPS data for antenna position // correct GPS data for antenna position
void CorrectGPSForAntennaOffset(gps_elements &gps_data); void CorrectGPSForAntennaOffset(gps_elements &gps_data) const;
// Variables // Variables
bool statesInitialised; // boolean true when filter states have been initialised bool statesInitialised; // boolean true when filter states have been initialised