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

OpticalFlow_Onboard: fix body rate calculation

Browse Source 
The correct integral of the body rate is (rate + last_rate)/2*dt
and not rate * dt
This commit is contained in:
Julien BERAUD 2016-01-14 16:58:46 +01:00 committed by Lucas De Marchi
parent 753f7d864a
commit 43307d95c8
2 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp
View File

@ -374,10 +374,12 @@ void OpticalFlow_Onboard::_run_optflow()
HAL_FLOW_PX4_FOCAL_LENGTH_MILLIPX;
_integration_timespan += video_frame.timestamp -
_last_video_frame.timestamp;
_gyro_x_integral += gyro_rate.x * (video_frame.timestamp -
_last_video_frame.timestamp);
_gyro_y_integral += gyro_rate.y * (video_frame.timestamp -
_last_video_frame.timestamp);
_gyro_x_integral += (gyro_rate.x + _last_gyro_rate.x) / 2.0f *
(video_frame.timestamp -
_last_video_frame.timestamp);
_gyro_y_integral += (gyro_rate.y + _last_gyro_rate.y) / 2.0f *
(video_frame.timestamp -
_last_video_frame.timestamp);
_surface_quality = qual;
_data_available = true;
pthread_mutex_unlock(&_mutex);
@ -385,6 +387,7 @@ void OpticalFlow_Onboard::_run_optflow()
/* give the last frame back to the video input driver */
_videoin->put_frame(_last_video_frame);
_last_video_frame = video_frame;
_last_gyro_rate = gyro_rate;
}
if (convert_buffer) {

1
libraries/AP_HAL_Linux/OpticalFlow_Onboard.h
View File

@ -57,4 +57,5 @@ private:
uint32_t _integration_timespan;
uint8_t _surface_quality;
AP_HAL::OpticalFlow::Gyro_Cb _get_gyro;
Vector3f _last_gyro_rate;
};