|
|
发表于 2014-4-2 10:15:07
|
显示全部楼层
嗯，官方说的这个DPI应该是在某个确定的高度下，还有如果每次高度变了就要重新标定对吧，我觉得这个应该有个关系在里面，看过APM的光流那段程序，当然他这里面有角度的变换，最后把高度信息也加进去了，还有一个系数，不过这个系数怎么来的没看明白。
- void AP_OpticalFlow::update_position(float roll, float pitch, float sin_yaw, float cos_yaw, float altitude)
- {
- float diff_roll = roll - _last_roll;
- float diff_pitch = pitch - _last_pitch;
- float change_x, change_y; // actual change in x, y coordinates
- // only update position if surface quality is good and angle is not
- // over 45 degrees
- if( surface_quality >= 10 && fabsf(roll) <= FORTYFIVE_DEGREES
- && fabsf(pitch) <= FORTYFIVE_DEGREES ) {
- altitude = max(altitude, 0);
- // change in position is actual change measured by sensor (i.e. dx, dy) minus expected change due to change in roll, pitch
- change_x = dx - (diff_roll * radians_to_pixels);
- change_y = dy - (-diff_pitch * radians_to_pixels);
- float avg_altitude = (altitude + _last_altitude)*0.5f;
- // convert raw change to horizontal movement in cm
- // perhaps this altitude should actually be the distance to the
- // ground? i.e. if we are very rolled over it should be longer?
- x_cm = -change_x * avg_altitude * conv_factor;
- // for example if you are leaned over at 45 deg the ground will
- // appear farther away and motion from opt flow sensor will be less
- y_cm = -change_y * avg_altitude * conv_factor;
- }
- _last_altitude = altitude;
- _last_roll = roll;
- _last_pitch = pitch;
- }
|
|
