AP_Compass: cope with AP_INERTIAL_SENSOR being 0

This commit is contained in:
Peter Barker 2023-07-12 15:40:32 +10:00 committed by Peter Barker
parent 78087da03c
commit eec43c204f
2 changed files with 9 additions and 2 deletions

View File

@ -126,6 +126,7 @@ AP_Compass_Backend *AP_Compass_HMC5843::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev
return sensor;
}
#if AP_INERTIALSENSOR_ENABLED
AP_Compass_Backend *AP_Compass_HMC5843::probe_mpu6000(enum Rotation rotation)
{
AP_InertialSensor &ins = *AP_InertialSensor::get_singleton();
@ -145,6 +146,7 @@ AP_Compass_Backend *AP_Compass_HMC5843::probe_mpu6000(enum Rotation rotation)
return sensor;
}
#endif
bool AP_Compass_HMC5843::init()
{
@ -488,6 +490,7 @@ AP_HAL::Device::PeriodicHandle AP_HMC5843_BusDriver_HALDevice::register_periodic
}
#if AP_INERTIALSENSOR_ENABLED
/* HMC5843 on an auxiliary bus of IMU driver */
AP_HMC5843_BusDriver_Auxiliary::AP_HMC5843_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id,
uint8_t addr)
@ -496,14 +499,12 @@ AP_HMC5843_BusDriver_Auxiliary::AP_HMC5843_BusDriver_Auxiliary(AP_InertialSensor
* Only initialize members. Fails are handled by configure or while
* getting the semaphore
*/
#if AP_INERTIALSENSOR_ENABLED
_bus = ins.get_auxiliary_bus(backend_id);
if (!_bus) {
return;
}
_slave = _bus->request_next_slave(addr);
#endif
}
AP_HMC5843_BusDriver_Auxiliary::~AP_HMC5843_BusDriver_Auxiliary()
@ -585,5 +586,6 @@ uint32_t AP_HMC5843_BusDriver_Auxiliary::get_bus_id(void) const
{
return _bus->get_bus_id();
}
#endif // AP_INERTIALSENSOR_ENABLED
#endif // AP_COMPASS_HMC5843_ENABLED

View File

@ -13,6 +13,7 @@
#include <AP_HAL/Device.h>
#include "AP_Compass_Backend.h"
#include <AP_InertialSensor/AP_InertialSensor_config.h>
class AuxiliaryBus;
class AuxiliaryBusSlave;
@ -26,7 +27,9 @@ public:
bool force_external,
enum Rotation rotation);
#if AP_INERTIALSENSOR_ENABLED
static AP_Compass_Backend *probe_mpu6000(enum Rotation rotation);
#endif
static constexpr const char *name = "HMC5843";
@ -124,6 +127,7 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
};
#if AP_INERTIALSENSOR_ENABLED
class AP_HMC5843_BusDriver_Auxiliary : public AP_HMC5843_BusDriver
{
public:
@ -153,5 +157,6 @@ private:
AuxiliaryBusSlave *_slave;
bool _started;
};
#endif // AP_INERTIALSENSOR_ENABLED
#endif // AP_COMPASS_HMC5843_ENABLED