Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: external nav pos offsets from AP_VisualOdom library

This commit is contained in:
Randy Mackay 2020-04-13 12:19:46 +09:00 committed by Andrew Tridgell
parent c39fef6c56
commit 8749f30c64
5 changed files with 43 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -5,6 +5,7 @@
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_VisualOdom/AP_VisualOdom.h>
#include <new>
/*
@ -1627,12 +1628,14 @@ void NavEKF2::getTimingStatistics(int8_t instance, struct ekf_timing &timing) co
* timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
* resetTime_ms : system time of the last position reset request (mSec)
*
* Sensor offsets are pulled directly from the AP_VisualOdom library
*
*/
void NavEKF2::writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
void NavEKF2::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
{
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeExtNavData(sensOffset, pos, quat, posErr, angErr, timeStamp_ms, resetTime_ms);
core[i].writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, resetTime_ms);
}
}
}

4
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -337,8 +337,10 @@ public:
* timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
* resetTime_ms : system time of the last position reset request (mSec)
*
* Sensor offsets are pulled directly from the AP_VisualOdom library
*
*/
void writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
/*
check if switching lanes will reduce the normalised

3
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -893,7 +893,7 @@ void NavEKF2_core::getTimingStatistics(struct ekf_timing &_timing)
memset(&timing, 0, sizeof(timing));
}
void NavEKF2_core::writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
{
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
@ -918,7 +918,6 @@ void NavEKF2_core::writeExtNavData(const Vector3f &sensOffset, const Vector3f &p
extNavDataNew.posErr = frontend->_gpsHorizPosNoise;
}
extNavDataNew.angErr = angErr;
extNavDataNew.body_offset = &sensOffset;
timeStamp_ms = timeStamp_ms - frontend->_extnavDelay_ms;
// Correct for the average intersampling delay due to the filter updaterate
timeStamp_ms -= localFilterTimeStep_ms/2;

31
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -120,10 +120,12 @@ void NavEKF2_core::ResetPosition(void)
lastRngBcnPassTime_ms = imuSampleTime_ms;
} else if (imuSampleTime_ms - extNavDataDelayed.time_ms < 250) {
// use external nav data as the third preference
stateStruct.position.x = extNavDataDelayed.pos.x;
stateStruct.position.y = extNavDataDelayed.pos.y;
ext_nav_elements extNavCorrected = extNavDataDelayed;
CorrectExtNavForSensorOffset(extNavCorrected.pos);
stateStruct.position.x = extNavCorrected.pos.x;
stateStruct.position.y = extNavCorrected.pos.y;
// set the variances from the external nav filter
P[7][7] = P[6][6] = sq(extNavDataDelayed.posErr);
P[7][7] = P[6][6] = sq(extNavCorrected.posErr);
}
}
for (uint8_t i=0; i<imu_buffer_length; i++) {
@ -275,6 +277,25 @@ void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data)
gps_data.hgt += posOffsetEarth.z;
}
// correct external navigation earth-frame position using sensor body-frame offset
void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3f &ext_position)
{
#if HAL_VISUALODOM_ENABLED
AP_VisualOdom *visual_odom = AP::visualodom();
if (visual_odom == nullptr) {
return;
}
const Vector3f &posOffsetBody = visual_odom->get_pos_offset() - accelPosOffset;
if (posOffsetBody.is_zero()) {
return;
}
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
ext_position.x -= posOffsetEarth.x;
ext_position.y -= posOffsetEarth.y;
ext_position.z -= posOffsetEarth.z;
#endif
}
/********************************************************
* FUSE MEASURED_DATA *
********************************************************/
@ -327,6 +348,10 @@ void NavEKF2_core::SelectVelPosFusion()
fuseVelData = false;
fuseHgtData = true;
fusePosData = true;
// correct for external navigation sensor position
CorrectExtNavForSensorOffset(extNavDataDelayed.pos);
velPosObs[3] = extNavDataDelayed.pos.x;
velPosObs[4] = extNavDataDelayed.pos.y;
velPosObs[5] = extNavDataDelayed.pos.z;

9
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -314,7 +314,6 @@ public:
/*
* Write position and quaternion data from an external navigation system
*
* sensOffset : position of the external navigation sensor in body frame (m)
* pos : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m)
* quat : quaternion desribing the rotation from navigation frame to body frame
* posErr : 1-sigma spherical position error (m)
@ -322,8 +321,10 @@ public:
* timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
* resetTime_ms : system time of the last position reset request (mSec)
*
* Sensor offsets are pulled directly from the AP_VisualOdom library
*
*/
void writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
// return true when external nav data is also being used as a yaw observation
bool isExtNavUsedForYaw(void);
@ -475,7 +476,6 @@ private:
Quaternion quat; // quaternion describing the rotation from navigation to body frame
float posErr; // spherical poition measurement error 1-std (m)
float angErr; // spherical angular measurement error 1-std (rad)
const Vector3f *body_offset;// pointer to XYZ position of the sensor in body frame (m)
uint32_t time_ms; // measurement timestamp (msec)
bool posReset; // true when the position measurement has been reset
};
@ -779,6 +779,9 @@ private:
// correct gps data for antenna position
void CorrectGPSForAntennaOffset(gps_elements &gps_data);
// correct external navigation earth-frame position using sensor body-frame offset
void CorrectExtNavForSensorOffset(Vector3f &ext_position);
// Length of FIFO buffers used for non-IMU sensor data.
// Must be larger than the time period defined by IMU_BUFFER_LENGTH
static const uint32_t OBS_BUFFER_LENGTH = 5;