mirror of https://github.com/ArduPilot/ardupilot
HAL_SITL: fixed sonar correction for attitude
This commit is contained in:
parent
50f492a69f
commit
5da5360dde
|
@ -69,17 +69,21 @@ uint16_t SITL_State::_ground_sonar(float altitude)
|
|||
|
||||
altitude = altitude - home_alt;
|
||||
|
||||
// adjust for apparent altitude with roll
|
||||
altitude *= cos(radians(_sitl->state.rollDeg)) * cos(radians(_sitl->state.pitchDeg));
|
||||
float voltage = 5.0f;
|
||||
if (fabsf(_sitl->state.rollDeg) < 90 &&
|
||||
fabsf(_sitl->state.pitchDeg) < 90) {
|
||||
// adjust for apparent altitude with roll
|
||||
altitude /= cos(radians(_sitl->state.rollDeg)) * cos(radians(_sitl->state.pitchDeg));
|
||||
|
||||
altitude += _sitl->sonar_noise * _rand_float();
|
||||
altitude += _sitl->sonar_noise * _rand_float();
|
||||
|
||||
// Altitude in in m, scaler in meters/volt
|
||||
float voltage = altitude / _sitl->sonar_scale;
|
||||
voltage = constrain_float(voltage, 0, 5.0f);
|
||||
// Altitude in in m, scaler in meters/volt
|
||||
voltage = altitude / _sitl->sonar_scale;
|
||||
voltage = constrain_float(voltage, 0, 5.0f);
|
||||
|
||||
if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f) {
|
||||
voltage = 5.0f;
|
||||
if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f) {
|
||||
voltage = 5.0f;
|
||||
}
|
||||
}
|
||||
|
||||
return 1023*(voltage / 5.0f);
|
||||
|
|
Loading…
Reference in New Issue