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

AP_NavEKF3: extnav data gets corrected boolean

Browse Source 
CorrectExtNavForSensorOffset and CorrectExtNavVelForSensorOffset use
corrected boolean to avoid correcting twice
This commit is contained in:
Randy Mackay 2020-10-22 19:55:50 +09:00 committed by Andrew Tridgell
parent a16de76f57
commit 2f1ae29a9c
3 changed files with 26 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -1054,7 +1054,8 @@ void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t
const ext_nav_vel_elements extNavVelNew {
vel,
err,
timeStamp_ms
timeStamp_ms,
false
};
storedExtNavVel.push(extNavVelNew);
}

28
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -345,8 +345,14 @@ void NavEKF3_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
}
// correct external navigation earth-frame position using sensor body-frame offset
void NavEKF3_core::CorrectExtNavForSensorOffset(Vector3f &ext_position)
void NavEKF3_core::CorrectExtNavForSensorOffset(ext_nav_elements &ext_nav_data)
{
// return immediately if already corrected
if (ext_nav_data.corrected) {
return;
}
ext_nav_data.corrected = true;
#if HAL_VISUALODOM_ENABLED
AP_VisualOdom *visual_odom = AP::visualodom();
if (visual_odom == nullptr) {
@ -357,15 +363,21 @@ void NavEKF3_core::CorrectExtNavForSensorOffset(Vector3f &ext_position)
return;
}
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
ext_position.x -= posOffsetEarth.x;
ext_position.y -= posOffsetEarth.y;
ext_position.z -= posOffsetEarth.z;
ext_nav_data.pos.x -= posOffsetEarth.x;
ext_nav_data.pos.y -= posOffsetEarth.y;
ext_nav_data.pos.z -= posOffsetEarth.z;
#endif
}
// correct external navigation earth-frame velocity using sensor body-frame offset
void NavEKF3_core::CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const
void NavEKF3_core::CorrectExtNavVelForSensorOffset(ext_nav_vel_elements &ext_nav_vel_data) const
{
// return immediately if already corrected
if (ext_nav_vel_data.corrected) {
return;
}
ext_nav_vel_data.corrected = true;
#if HAL_VISUALODOM_ENABLED
AP_VisualOdom *visual_odom = AP::visualodom();
if (visual_odom == nullptr) {
@ -377,7 +389,7 @@ void NavEKF3_core::CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const
}
// TODO use a filtered angular rate with a group delay that matches the sensor delay
const Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
ext_velocity += get_vel_correction_for_sensor_offset(posOffsetBody, prevTnb, angRate);
ext_nav_vel_data.vel += get_vel_correction_for_sensor_offset(posOffsetBody, prevTnb, angRate);
#endif
}
@ -401,11 +413,11 @@ void NavEKF3_core::SelectVelPosFusion()
// Check for data at the fusion time horizon
extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
if (extNavDataToFuse) {
CorrectExtNavForSensorOffset(extNavDataDelayed.pos);
CorrectExtNavForSensorOffset(extNavDataDelayed);
}
extNavVelToFuse = storedExtNavVel.recall(extNavVelDelayed, imuDataDelayed.time_ms);
if (extNavVelToFuse) {
CorrectExtNavVelForSensorOffset(extNavVelDelayed.vel);
CorrectExtNavVelForSensorOffset(extNavVelDelayed);
}
// Read GPS data from the sensor

6
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -628,12 +628,14 @@ private:
float posErr; // spherical poition measurement error 1-std (m)
uint32_t time_ms; // measurement timestamp (msec)
bool posReset; // true when the position measurement has been reset
bool corrected; // true when the position has been corrected for sensor position
};
struct ext_nav_vel_elements {
Vector3f vel; // velocity in NED (m/s)
float err; // velocity measurement error (m/s)
uint32_t time_ms; // measurement timestamp (msec)
bool corrected; // true when the velocity has been corrected for sensor position
};
// bias estimates for the IMUs that are enabled but not being used
@ -945,10 +947,10 @@ private:
void CorrectGPSForAntennaOffset(gps_elements &gps_data) const;
// correct external navigation earth-frame position using sensor body-frame offset
void CorrectExtNavForSensorOffset(Vector3f &ext_position);
void CorrectExtNavForSensorOffset(ext_nav_elements &ext_nav_data);
// correct external navigation earth-frame velocity using sensor body-frame offset
void CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const;
void CorrectExtNavVelForSensorOffset(ext_nav_vel_elements &ext_nav_vel_data) const;
// Runs the IMU prediction step for an independent GSF yaw estimator algorithm
// that uses IMU, GPS horizontal velocity and optionally true airspeed data.