25 #define REG_COMPANY_ID 0x00 26 #define REG_DEVICE_ID 0x01 36 #define REG_CNTL1 0x30 37 #define REG_CNTL2 0x31 38 #define REG_CNTL3 0x32 40 #define REG_ICM_WHOAMI 0x00 41 #define REG_ICM_PWR_MGMT_1 0x06 42 #define REG_ICM_INT_PIN_CFG 0x0f 44 #define ICM_WHOAMI_VAL 0xEA 62 if (!sensor || !sensor->
init()) {
86 if (!sensor || !sensor->
init()) {
183 const float to_utesla = 4912.0f / 32752.0f;
184 const float utesla_to_milliGauss = 10;
185 const float range_scale = to_utesla * utesla_to_milliGauss;
191 goto check_registers;
195 goto check_registers;
198 field(data.magx * range_scale, data.magy * range_scale, data.magz * range_scale);
int printf(const char *fmt,...)
AP_HAL::OwnPtr< AP_HAL::Device > dev_icm
void set_external(uint8_t instance, bool external)
virtual PeriodicHandle register_periodic_callback(uint32_t period_usec, PeriodicCb)=0
uint32_t get_bus_id(void) const
virtual AP_HAL::Semaphore * get_semaphore()=0
void rotate_field(Vector3f &mag, uint8_t instance)
void publish_filtered_field(const Vector3f &mag, uint8_t instance)
void correct_field(Vector3f &mag, uint8_t i)
#define HAL_SEMAPHORE_BLOCK_FOREVER
virtual bool take(uint32_t timeout_ms) WARN_IF_UNUSED=0
AP_HAL::OwnPtr< AP_HAL::Device > dev
void set_dev_id(uint8_t instance, uint32_t dev_id)
static AP_Compass_Backend * probe(Compass &compass, AP_HAL::OwnPtr< AP_HAL::I2CDevice > dev, bool force_external=false, enum Rotation rotation=ROTATION_NONE)
bool setup_checked_registers(uint8_t num_regs, uint8_t frequency=10)
void fail(const char *why)
virtual void delay(uint16_t ms)=0
void set_device_type(uint8_t devtype)
virtual bool take_nonblocking() WARN_IF_UNUSED=0
static AP_HAL::OwnPtr< AP_HAL::Device > dev
uint8_t register_compass(void) const
AP_Compass_AK09916(Compass &compass, AP_HAL::OwnPtr< AP_HAL::Device > dev, AP_HAL::OwnPtr< AP_HAL::Device > dev_icm, bool force_external, enum Rotation rotation, enum bus_type bus_type)
#define REG_ICM_PWR_MGMT_1
void set_rotation(uint8_t instance, enum Rotation rotation)
bool check_next_register(void)
bool read_registers(uint8_t first_reg, uint8_t *recv, uint32_t recv_len)
#define FUNCTOR_BIND_MEMBER(func, rettype,...)
void publish_raw_field(const Vector3f &mag, uint8_t instance)
#define REG_ICM_INT_PIN_CFG
bool write_register(uint8_t reg, uint8_t val, bool checked=false)
AP_HAL::Scheduler * scheduler
static AP_Compass_Backend * probe_ICM20948(Compass &compass, AP_HAL::OwnPtr< AP_HAL::I2CDevice > dev, AP_HAL::OwnPtr< AP_HAL::I2CDevice > dev_icm, bool force_external=false, enum Rotation rotation=ROTATION_NONE)