AP_Compass: fixed custom orientation in 3 compass drivers

this prevents an internal error and mag failure with DroneCAN, MSP or
ExternalAHRS compasses
This commit is contained in:
Andrew Tridgell 2022-05-02 08:21:51 +10:00
parent 5367833e22
commit 3c6e4d907c
3 changed files with 3 additions and 0 deletions

View File

@ -25,6 +25,7 @@ AP_Compass_ExternalAHRS::AP_Compass_ExternalAHRS(uint8_t port)
set_dev_id(instance, devid);
set_external(instance, true);
set_rotation(instance, ROTATION_NONE);
}
void AP_Compass_ExternalAHRS::handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt)

View File

@ -27,6 +27,7 @@ AP_Compass_MSP::AP_Compass_MSP(uint8_t _msp_instance)
set_dev_id(instance, devid);
set_external(instance, true);
set_rotation(instance, ROTATION_NONE);
}
void AP_Compass_MSP::handle_msp(const MSP::msp_compass_data_message_t &pkt)

View File

@ -101,6 +101,7 @@ bool AP_Compass_UAVCAN::init()
set_dev_id(_instance, _devid);
set_external(_instance, true);
set_rotation(_instance, ROTATION_NONE);
AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "AP_Compass_UAVCAN loaded\n\r");
return true;