Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: make velocity and rotmat available to the optical flow simulator

Browse Source
This commit is contained in:
Andrew Tridgell 2015-01-03 10:57:53 +11:00
parent aff01c6d09
commit 51cd64b3b2
1 changed files with 13 additions and 1 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

@ -62,11 +62,23 @@ void SITL_State::_update_flow(void)
float height_agl = _sitl->state.altitude - terrain_height_amsl;
// NED velocity vector in m/s
Vector3f velocity(_sitl->state.speedN,
_sitl->state.speedE,
_sitl->state.speedD);
// a rotation matrix following DCM conventions
Matrix3f rotmat;
rotmat.from_euler(radians(_sitl->state.rollDeg),
radians(_sitl->state.pitchDeg),
radians(_sitl->state.yawDeg));
state.device_id = 1;
state.surface_quality = 0;
// rubbish calculation for Paul to fill in
state.flowRate = Vector2f(gyro.x, gyro.y) * height_agl;
state.flowRate = Vector2f(gyro.x, gyro.y) * height_agl * velocity.length();
state.bodyRate = Vector2f(gyro.x, gyro.y) * height_agl;
_optical_flow->setHIL(state);