Compass: enable compass offsets for HIL compass

this makes the null_offsets algorithm have an effect for the HIL
compass that is used for SITL, which makes for easier testing of
compass calibration
This commit is contained in:
Andrew Tridgell 2012-02-15 22:33:50 +11:00
parent 048aed5427
commit 62326c0f72
1 changed files with 5 additions and 5 deletions

View File

@ -23,9 +23,9 @@ bool AP_Compass_HIL::read()
//
void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
{
// TODO: map floats to raw
mag_x = _mag_x;
mag_y = _mag_y;
mag_z = _mag_z;
healthy = true;
Vector3f ofs = _offset.get();
mag_x = _mag_x + ofs.x;
mag_y = _mag_y + ofs.y;
mag_z = _mag_z + ofs.z;
healthy = true;
}