Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_SITL: rangefinder const correctness

Browse Source
This commit is contained in:
Pierre Kancir 2017-03-03 11:19:18 +01:00 committed by Andrew Tridgell
parent 3f045a4905
commit 22dd81e298
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_HAL_SITL/sitl_rangefinder.cpp
View File

@ -30,7 +30,7 @@ void SITL_State::_update_rangefinder(float range_value)
} }
// sensor position offset in body frame // sensor position offset in body frame
Vector3f relPosSensorBF = _sitl->rngfnd_pos_offset; const Vector3f relPosSensorBF = _sitl->rngfnd_pos_offset;
// adjust altitude for position of the sensor on the vehicle if position offset is non-zero // adjust altitude for position of the sensor on the vehicle if position offset is non-zero
if (!relPosSensorBF.is_zero()) { if (!relPosSensorBF.is_zero()) {
@ -38,7 +38,7 @@ void SITL_State::_update_rangefinder(float range_value)
Matrix3f rotmat; Matrix3f rotmat;
_sitl->state.quaternion.rotation_matrix(rotmat); _sitl->state.quaternion.rotation_matrix(rotmat);
// rotate the offset into earth frame // rotate the offset into earth frame
Vector3f relPosSensorEF = rotmat * relPosSensorBF; const Vector3f relPosSensorEF = rotmat * relPosSensorBF;
// correct the altitude at the sensor // correct the altitude at the sensor
altitude -= relPosSensorEF.z; altitude -= relPosSensorEF.z;
} }