Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: fixed sonar correction for attitude

This commit is contained in:
Andrew Tridgell 2014-08-26 21:35:15 +10:00
parent 50f492a69f
commit 5da5360dde
1 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
View File

@ -69,18 +69,22 @@ uint16_t SITL_State::_ground_sonar(float altitude)
altitude = altitude - home_alt;
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 /= cos(radians(_sitl->state.rollDeg)) * cos(radians(_sitl->state.pitchDeg));
altitude += _sitl->sonar_noise * _rand_float();
// Altitude in in m, scaler in meters/volt
float voltage = altitude / _sitl->sonar_scale;
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;
}
}
return 1023*(voltage / 5.0f);
}