AP_Compass: default to right mag offsets for SITL

This commit is contained in:
Andrew Tridgell 2018-07-13 09:35:42 +10:00
parent 2fe2c8fbda
commit 7ec6522b9d
1 changed files with 3 additions and 0 deletions

View File

@ -13,6 +13,9 @@ AP_Compass_SITL::AP_Compass_SITL(Compass &compass):
if (_sitl != nullptr) {
_compass._setup_earth_field();
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
// default offsets to correct value
compass.set_offsets(i, _sitl->mag_ofs);
_compass_instance[i] = register_compass();
set_dev_id(_compass_instance[i], AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 0, DEVTYPE_SITL));