Browse Source

AP_HAL_SITL: rangefinder const correctness

master
Pierre Kancir 8 years ago committed by Andrew Tridgell
parent
commit
22dd81e298
  1. 4
      libraries/AP_HAL_SITL/sitl_rangefinder.cpp

4
libraries/AP_HAL_SITL/sitl_rangefinder.cpp

@ -30,7 +30,7 @@ void SITL_State::_update_rangefinder(float range_value) @@ -30,7 +30,7 @@ void SITL_State::_update_rangefinder(float range_value)
}
// 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
if (!relPosSensorBF.is_zero()) {
@ -38,7 +38,7 @@ void SITL_State::_update_rangefinder(float range_value) @@ -38,7 +38,7 @@ void SITL_State::_update_rangefinder(float range_value)
Matrix3f rotmat;
_sitl->state.quaternion.rotation_matrix(rotmat);
// rotate the offset into earth frame
Vector3f relPosSensorEF = rotmat * relPosSensorBF;
const Vector3f relPosSensorEF = rotmat * relPosSensorBF;
// correct the altitude at the sensor
altitude -= relPosSensorEF.z;
}

Loading…
Cancel
Save