Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-20 15:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: save some memory in the position offsets in EKF2

Browse Source 
we don't need to copy that vector3f for every sample. A uint8_t does
the job
This commit is contained in:
Andrew Tridgell 2016-10-27 15:47:26 +11:00
parent a05d216ce4
commit 82365f8670
6 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -1016,7 +1016,7 @@ bool NavEKF2::use_compass(void) const
// The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
// posOffset is the XYZ flow sensor position in the body frame in m // posOffset is the XYZ flow sensor position in the body frame in m
void NavEKF2::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, Vector3f &posOffset) void NavEKF2::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset)
{ {
if (core) { if (core) {
for (uint8_t i=0; i<num_cores; i++) { for (uint8_t i=0; i<num_cores; i++) {

2
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -201,7 +201,7 @@ public:
// The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
// posOffset is the XYZ flow sensor position in the body frame in m // posOffset is the XYZ flow sensor position in the body frame in m
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, Vector3f &posOffset); void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset);
// return data for debugging optical flow fusion for the specified instance // return data for debugging optical flow fusion for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance // An out of range instance (eg -1) returns data for the the primary instance

10
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -78,7 +78,7 @@ void NavEKF2_core::readRangeFinder(void)
rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd); rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd);
// get position in body frame for the current sensor // get position in body frame for the current sensor
rangeDataNew.body_offset = frontend->_rng.get_pos_offset(sensorIndex); rangeDataNew.sensor_idx = sensorIndex;
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
storedRange.push(rangeDataNew); storedRange.push(rangeDataNew);
@ -110,7 +110,7 @@ void NavEKF2_core::readRangeFinder(void)
// write the raw optical flow measurements // write the raw optical flow measurements
// this needs to be called externally. // this needs to be called externally.
void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, Vector3f &posOffset) void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset)
{ {
// The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
// The PX4Flow sensor outputs flow rates with the following axis and sign conventions: // The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
@ -154,7 +154,7 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
// note correction for different axis and sign conventions used by the px4flow sensor // note correction for different axis and sign conventions used by the px4flow sensor
ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec) ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
// write the flow sensor position in body frame // write the flow sensor position in body frame
ofDataNew.body_offset = posOffset; ofDataNew.body_offset = &posOffset;
// write flow rate measurements corrected for body rates // write flow rate measurements corrected for body rates
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x; ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y; ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;
@ -418,8 +418,8 @@ void NavEKF2_core::readGpsData()
// Prevent time delay exceeding age of oldest IMU data in the buffer // Prevent time delay exceeding age of oldest IMU data in the buffer
gpsDataNew.time_ms = MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms); gpsDataNew.time_ms = MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms);
// Get the GPS position offset in body frame // Get which GPS we are using for position information
gpsDataNew.body_offset = _ahrs->get_gps().get_antenna_offset(); gpsDataNew.sensor_idx = _ahrs->get_gps().primary_sensor();
// read the NED velocity from the GPS // read the NED velocity from the GPS
gpsDataNew.vel = _ahrs->get_gps().velocity(); gpsDataNew.vel = _ahrs->get_gps().velocity();

2
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -306,7 +306,7 @@ void NavEKF2_core::FuseOptFlow()
// correct range for flow sensor offset body frame position offset // correct range for flow sensor offset body frame position offset
// the corrected value is the predicted range from the sensor focal point to the // the corrected value is the predicted range from the sensor focal point to the
// centre of the image on the ground assuming flat terrain // centre of the image on the ground assuming flat terrain
Vector3f posOffsetBody = ofDataDelayed.body_offset - accelPosOffset; Vector3f posOffsetBody = (*ofDataDelayed.body_offset) - accelPosOffset;
if (!posOffsetBody.is_zero()) { if (!posOffsetBody.is_zero()) {
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
range -= posOffsetEarth.z / prevTnb.c.z; range -= posOffsetEarth.z / prevTnb.c.z;

4
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -198,7 +198,7 @@ void NavEKF2_core::SelectVelPosFusion()
fusePosData = true; fusePosData = true;
// 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
Vector3f posOffsetBody = gpsDataDelayed.body_offset - accelPosOffset; Vector3f posOffsetBody = _ahrs->get_gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
if (!posOffsetBody.is_zero()) { if (!posOffsetBody.is_zero()) {
if (fuseVelData) { if (fuseVelData) {
// TODO use a filtered angular rate with a group delay that matches the GPS delay // TODO use a filtered angular rate with a group delay that matches the GPS delay
@ -652,7 +652,7 @@ void NavEKF2_core::selectHeightForFusion()
// the corrected reading is the reading that would have been taken if the sensor was // the corrected reading is the reading that would have been taken if the sensor was
// co-located with the IMU // co-located with the IMU
if (rangeDataToFuse) { if (rangeDataToFuse) {
Vector3f posOffsetBody = rangeDataDelayed.body_offset - accelPosOffset; Vector3f posOffsetBody = frontend->_rng.get_pos_offset(rangeDataDelayed.sensor_idx) - accelPosOffset;
if (!posOffsetBody.is_zero()) { if (!posOffsetBody.is_zero()) {
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z; rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;

8
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -188,7 +188,7 @@ public:
// The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
// posOffset is the XYZ flow sensor position in the body frame in m // posOffset is the XYZ flow sensor position in the body frame in m
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, Vector3f &posOffset); void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset);
// return data for debugging optical flow fusion // return data for debugging optical flow fusion
void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const; void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const;
@ -374,7 +374,7 @@ private:
float hgt; // 2 float hgt; // 2
Vector3f vel; // 3..5 Vector3f vel; // 3..5
uint32_t time_ms; // 6 uint32_t time_ms; // 6
Vector3f body_offset; // 7..9 uint8_t sensor_idx; // 7..9
}; };
struct mag_elements { struct mag_elements {
@ -390,7 +390,7 @@ private:
struct range_elements { struct range_elements {
float rng; // 0 float rng; // 0
uint32_t time_ms; // 1 uint32_t time_ms; // 1
Vector3f body_offset; // 2..4 uint8_t sensor_idx; // 2
}; };
struct tas_elements { struct tas_elements {
@ -402,8 +402,8 @@ private:
Vector2f flowRadXY; // 0..1 Vector2f flowRadXY; // 0..1
Vector2f flowRadXYcomp; // 2..3 Vector2f flowRadXYcomp; // 2..3
uint32_t time_ms; // 4 uint32_t time_ms; // 4
Vector3f body_offset; // 5..7
Vector3f bodyRadXYZ; //8..10 Vector3f bodyRadXYZ; //8..10
const Vector3f *body_offset;// 5..7
}; };
// update the navigation filter status // update the navigation filter status