aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Add rangefinder body position offset to data buffer

Browse Source
This commit is contained in:
priseborough 2016-10-12 08:27:43 +11:00 committed by Andrew Tridgell
parent 3148ad4623
commit 92c086b40e
2 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -77,6 +77,9 @@ void NavEKF2_core::readRangeFinder(void)
// limit the measured range to be no less than the on-ground range // limit the measured range to be no less than the on-ground range
rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd); rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd);
// get position in body frame for the current sensor
rangeDataNew.body_offset = frontend->_rng.get_pos_offset(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);

1
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -390,6 +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
}; };
struct tas_elements { struct tas_elements {