diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index dc7efcb57d0..a408da67648 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -1373,7 +1373,7 @@ void Compass::_detect_backends(void) #if AP_COMPASS_EXTERNALAHRS_ENABLED const int8_t serial_port = AP::externalAHRS().get_port(AP_ExternalAHRS::AvailableSensor::COMPASS); if (serial_port >= 0) { - ADD_BACKEND(DRIVER_EXTERNALAHRS, NEW_NOTHROW AP_Compass_ExternalAHRS(serial_port)); + ADD_BACKEND(DRIVER_EXTERNALAHRS, AP_Compass_ExternalAHRS::probe(serial_port)); } #endif diff --git a/libraries/AP_Compass/AP_Compass_ExternalAHRS.cpp b/libraries/AP_Compass/AP_Compass_ExternalAHRS.cpp index 87a57cd303d..af305afeb85 100644 --- a/libraries/AP_Compass/AP_Compass_ExternalAHRS.cpp +++ b/libraries/AP_Compass/AP_Compass_ExternalAHRS.cpp @@ -18,13 +18,20 @@ #if AP_COMPASS_EXTERNALAHRS_ENABLED -AP_Compass_ExternalAHRS::AP_Compass_ExternalAHRS(uint8_t port) +AP_Compass_Backend *AP_Compass_ExternalAHRS::probe(uint8_t port) { auto devid = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL,port,0,0); - register_compass(devid, instance); - - set_dev_id(instance, devid); - set_external(instance, true); + auto *ret = NEW_NOTHROW AP_Compass_ExternalAHRS(); + if (ret == nullptr) { + return nullptr; + } + if (!ret->register_compass(devid, ret->instance)) { + delete ret; + return nullptr; + } + ret->set_dev_id(ret->instance, devid); + ret->set_external(ret->instance, true); + return ret; } void AP_Compass_ExternalAHRS::handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt) diff --git a/libraries/AP_Compass/AP_Compass_ExternalAHRS.h b/libraries/AP_Compass/AP_Compass_ExternalAHRS.h index 8162e7646ca..29e3367b366 100644 --- a/libraries/AP_Compass/AP_Compass_ExternalAHRS.h +++ b/libraries/AP_Compass/AP_Compass_ExternalAHRS.h @@ -11,7 +11,9 @@ class AP_Compass_ExternalAHRS : public AP_Compass_Backend { public: - AP_Compass_ExternalAHRS(uint8_t instance); + using AP_Compass_Backend::AP_Compass_Backend; + + static AP_Compass_Backend *probe(uint8_t port); void read(void) override;