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

AP_OpticalFlow: SITL driver applies _FX/YSCALER params

This commit is contained in:
Randy Mackay 2021-08-17 21:28:37 +09:00
parent 2aa17916b1
commit 9313535a35
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp
View File

@ -86,12 +86,17 @@ void AP_OpticalFlow_SITL::update(void)
// correct relative velocity for rotation rates and sensor offset
relVelSensor += gyro % posRelSensorBF;
// scaling based on parameters
const Vector2f flowScaler = _flowScaler();
const float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
const float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
// 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
// poll to provide a delta angle across the interface
state.flowRate.x = -relVelSensor.y/range + gyro.x + _sitl->flow_noise * rand_float();
state.flowRate.y = relVelSensor.x/range + gyro.y + _sitl->flow_noise * rand_float();
state.flowRate.x = (-relVelSensor.y/range + gyro.x + _sitl->flow_noise * rand_float()) * flowScaleFactorX;
state.flowRate.y = (relVelSensor.x/range + gyro.y + _sitl->flow_noise * rand_float()) * flowScaleFactorY;
// The flow sensors body rates are assumed to be the same as the vehicle body rates (ie no misalignment)
// Note - these are instantaneous values. The sensor sums these values across the interval from the last