Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: fixed units of body frame conversion for optical flow

Browse Source
This commit is contained in:
Andrew Tridgell 2015-01-03 15:43:30 +11:00
parent bf4ebcfda1
commit 023c42593f
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp
View File

@ -38,11 +38,11 @@ void SITL_State::_update_flow(void)
}
// convert roll rates to body frame
SITL::convert_body_frame(radians(_sitl->state.rollDeg),
radians(_sitl->state.pitchDeg),
radians(_sitl->state.rollRate),
radians(_sitl->state.pitchRate),
radians(_sitl->state.yawRate),
SITL::convert_body_frame(_sitl->state.rollDeg,
_sitl->state.pitchDeg,
_sitl->state.rollRate,
_sitl->state.pitchRate,
_sitl->state.yawRate,
&p, &q, &r);
gyro(p, q, r);
@ -75,7 +75,7 @@ void SITL_State::_update_flow(void)
state.device_id = 1;
state.surface_quality = 0;
state.surface_quality = 51;
// estimate range to centre of image
float range;
@ -86,7 +86,7 @@ void SITL_State::_update_flow(void)
}
// Calculate relative velocity in sensor frame assuming no misalignment between sensor and vehicle body axes
Vector3f relVelSensor = rotmat*velocity;
Vector3f relVelSensor = rotmat.mul_transpose(velocity);
// Divide velocity by range and add body rates to get predicted sensed angular
// optical rates relative to X and Y sensor axes assuming no misalignment or scale