|
|
|
@ -91,8 +91,8 @@ void AP_OpticalFlow_SITL::update(void)
@@ -91,8 +91,8 @@ void AP_OpticalFlow_SITL::update(void)
|
|
|
|
|
// 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; |
|
|
|
|
state.flowRate.y = relVelSensor.x/range + gyro.y; |
|
|
|
|
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(); |
|
|
|
|
|
|
|
|
|
// 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
|
|
|
|
|