AP_OpticalFlow: apply yaw for flow in all drivers
This commit is contained in:
parent
06dfbc3e09
commit
22717f23be
@ -77,6 +77,8 @@ void AP_OpticalFlow_Onboard::update()
|
|||||||
|
|
||||||
state.bodyRate.y = 1.0f / float(data_frame.delta_time) *
|
state.bodyRate.y = 1.0f / float(data_frame.delta_time) *
|
||||||
data_frame.gyro_y_integral;
|
data_frame.gyro_y_integral;
|
||||||
|
|
||||||
|
_applyYaw(state.flowRate);
|
||||||
} else {
|
} else {
|
||||||
state.flowRate.zero();
|
state.flowRate.zero();
|
||||||
state.bodyRate.zero();
|
state.bodyRate.zero();
|
||||||
|
@ -117,6 +117,8 @@ void AP_OpticalFlow_SITL::update(void)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
_applyYaw(state.flowRate);
|
||||||
|
|
||||||
// copy results to front end
|
// copy results to front end
|
||||||
_update_frontend(state);
|
_update_frontend(state);
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user