Browse Source

AP_OpticalFlow: apply yaw for flow in all drivers

mission-4.1.18
Andrew Tridgell 8 years ago
parent
commit
22717f23be
  1. 2
      libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.cpp
  2. 2
      libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp

2
libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.cpp

@ -77,6 +77,8 @@ void AP_OpticalFlow_Onboard::update() @@ -77,6 +77,8 @@ void AP_OpticalFlow_Onboard::update()
state.bodyRate.y = 1.0f / float(data_frame.delta_time) *
data_frame.gyro_y_integral;
_applyYaw(state.flowRate);
} else {
state.flowRate.zero();
state.bodyRate.zero();

2
libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp

@ -117,6 +117,8 @@ void AP_OpticalFlow_SITL::update(void) @@ -117,6 +117,8 @@ void AP_OpticalFlow_SITL::update(void)
}
}
_applyYaw(state.flowRate);
// copy results to front end
_update_frontend(state);
}

Loading…
Cancel
Save