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

HAL_SITL: fix for multi-compass SITL

This commit is contained in:
Andrew Tridgell 2015-05-15 14:04:12 +10:00
parent 91b4ba3588
commit 3370ec62a1
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_HAL_SITL/sitl_compass.cpp
View File

@ -39,10 +39,11 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
if (yawDeg < -180.0f) {
yawDeg += 360.0f;
}
_compass->setHIL(radians(rollDeg), radians(pitchDeg), radians(yawDeg));
_compass->setHIL(0, radians(rollDeg), radians(pitchDeg), radians(yawDeg));
_compass->setHIL(1, radians(rollDeg), radians(pitchDeg), radians(yawDeg));
Vector3f noise = _rand_vec3f() * _sitl->mag_noise;
Vector3f motor = _sitl->mag_mot.get() * _current;
Vector3f new_mag_data = _compass->getHIL() + noise + motor;
Vector3f new_mag_data = _compass->getHIL(0) + noise + motor;
uint32_t now = hal.scheduler->millis();
// add delay
@ -77,7 +78,8 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
new_mag_data -= _sitl->mag_ofs.get();
_compass->setHIL(new_mag_data);
_compass->setHIL(0, new_mag_data);
_compass->setHIL(1, new_mag_data);
}
#endif