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

AP_HAL_SITL: simulate optical flow sensor body frame position offset

Browse Source
This commit is contained in:
priseborough 2016-10-15 10:43:56 +11:00 committed by Andrew Tridgell
parent 27dab27565
commit 3afe2b1694
1 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_HAL_SITL/sitl_optical_flow.cpp
View File

@ -69,10 +69,21 @@ void SITL_State::_update_flow(void)
state.device_id = 1;
state.surface_quality = 51;
// sensor position offset in body frame
Vector3f posRelSensorBF = _sitl->optflow_pos_offset;
// for effiency only do offset correction if offset is non-zero
bool correctForOffset = !posRelSensorBF.is_zero();
// estimate range to centre of image
float range;
if (rotmat.c.z > 0.05f && height_agl() > 0) {
range = height_agl() / rotmat.c.z;
if (!correctForOffset) {
range = height_agl() / rotmat.c.z;
} else {
Vector3f relPosSensorEF = rotmat * posRelSensorBF;
range = (height_agl() - relPosSensorEF.z) / rotmat.c.z;
}
} else {
range = 1e38f;
}
@ -80,6 +91,11 @@ void SITL_State::_update_flow(void)
// Calculate relative velocity in sensor frame assuming no misalignment between sensor and vehicle body axes
Vector3f relVelSensor = rotmat.mul_transpose(velocity);
// correct relative velocity for rotation rates and sensor offset
if (correctForOffset) {
relVelSensor += gyro % posRelSensorBF;
}
// Divide velocity by range and add body rates to get predicted sensed angular
// optical rates relative to X and Y sensor axes assuming no misalignment or scale
// factor error. Note - these are instantaneous values. The sensor sums these values across the interval from the last