Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -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
zeroRows(P,3,4);
zeroCols(P,3,4);
gps_elements gps_corrected = gpsDataNew;
CorrectGPSForAntennaOffset(gps_corrected);
if (PV_AidingMode != AID_ABSOLUTE) {
stateStruct.velocity.zero();
@ -39,6 +36,9 @@ void NavEKF2_core::ResetVelocity(void)
} else {
// reset horizontal velocity states to the GPS velocity if available
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.y = gps_corrected.vel.y;
// set the variances using the reported GPS speed accuracy
@ -85,9 +85,6 @@ void NavEKF2_core::ResetPosition(void)
// reset the corresponding covariances
zeroRows(P,6,7);
zeroCols(P,6,7);
gps_elements gps_corrected = gpsDataNew;
CorrectGPSForAntennaOffset(gps_corrected);
if (PV_AidingMode != AID_ABSOLUTE) {
// reset all position state history to the last known position
@ -98,6 +95,9 @@ void NavEKF2_core::ResetPosition(void)
} else {
// Use GPS data as first preference if fresh data is available
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
last_gps_idx = gps_corrected.sensor_idx;
// 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
*/
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;
if (posOffsetBody.is_zero()) {

2
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -777,7 +777,7 @@ private:
void updateTimingStatistics(void);
// 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
void CorrectExtNavForSensorOffset(Vector3f &ext_position);