Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: 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:00 +09:00 committed by Andrew Tridgell
parent 3397bce235
commit 85704e4989
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_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -28,9 +28,6 @@ void NavEKF2_core::ResetVelocity(void)
// reset the corresponding covariances // reset the corresponding covariances
zeroRows(P,3,4); zeroRows(P,3,4);
zeroCols(P,3,4); zeroCols(P,3,4);
gps_elements gps_corrected = gpsDataNew;
CorrectGPSForAntennaOffset(gps_corrected);
if (PV_AidingMode != AID_ABSOLUTE) { if (PV_AidingMode != AID_ABSOLUTE) {
stateStruct.velocity.zero(); stateStruct.velocity.zero();
@ -39,6 +36,9 @@ void NavEKF2_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) { if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
// 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
@ -85,9 +85,6 @@ void NavEKF2_core::ResetPosition(void)
// reset the corresponding covariances // reset the corresponding covariances
zeroRows(P,6,7); zeroRows(P,6,7);
zeroCols(P,6,7); zeroCols(P,6,7);
gps_elements gps_corrected = gpsDataNew;
CorrectGPSForAntennaOffset(gps_corrected);
if (PV_AidingMode != AID_ABSOLUTE) { if (PV_AidingMode != AID_ABSOLUTE) {
// reset all position state history to the last known position // reset all position state history to the last known position
@ -98,6 +95,9 @@ void NavEKF2_core::ResetPosition(void)
} else { } else {
// 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) { if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
// 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
@ -253,7 +253,7 @@ bool NavEKF2_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 NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
{ {
const Vector3f &posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset; const Vector3f &posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
if (posOffsetBody.is_zero()) { if (posOffsetBody.is_zero()) {

2
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -777,7 +777,7 @@ private:
void updateTimingStatistics(void); void updateTimingStatistics(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;
// correct external navigation earth-frame position using sensor body-frame offset // correct external navigation earth-frame position using sensor body-frame offset
void CorrectExtNavForSensorOffset(Vector3f &ext_position); void CorrectExtNavForSensorOffset(Vector3f &ext_position);