void hmc5883l_init()
{
uint8_t send_rec[2];
hmc5883l_read(HMC5883L_ADDR, HMC5883L_IDEN_A, send_rec, 1);
if(send_rec[0] != 0x48)
{
xprintf("hmc5883l address error:%X %X %X !\r\n", HMC5883L_ADDR, HMC5883L_IDEN_A, send_rec[0]);
while(1);
}
xprintf("hmc5883l IDEN_A:%X \r\n", send_rec[0]);
send_rec[0] = 0x70;
hmc5883l_write(HMC5883L_ADDR, HMC5883L_CRA, send_rec, 1); // 8-average, 15 Hz default, normal measurement
send_rec[0] = 0x00;
hmc5883l_write(HMC5883L_ADDR, HMC5883L_CRB, send_rec, 1); // Gain = 0, or any other desired gain
send_rec[0] = 0x00;
hmc5883l_write(HMC5883L_ADDR, HMC5883L_MODE, send_rec, 1); // Continuous-measurement mode
nrf_delay_ms(67);
}
static struct device_t * compass_hmc5883l_probe(struct driver_t * drv, struct dtnode_t * n)
{
struct compass_hmc5883l_pdata_t * pdat;
struct compass_t * c;
struct device_t * dev;
struct i2c_device_t * i2cdev;
u8_t ida, idb, idc;
i2cdev = i2c_device_alloc(dt_read_string(n, "i2c-bus", NULL), dt_read_int(n, "slave-address", 0x1e), 0);
if(!i2cdev)
return NULL;
if(hmc5883l_read(i2cdev, REG_IDA, &ida)
&& hmc5883l_read(i2cdev, REG_IDB, &idb)
&& hmc5883l_read(i2cdev, REG_IDC, &idc)
&& (ida == 0x48)
&& (idb == 0x34)
&& (idc == 0x33))
{
hmc5883l_write(i2cdev, REG_CFGA, 0x70);
hmc5883l_write(i2cdev, REG_CFGB, 0x20);
hmc5883l_write(i2cdev, REG_MODE, 0x00);
}
else
{
i2c_device_free(i2cdev);
return NULL;
}
pdat = malloc(sizeof(struct compass_hmc5883l_pdata_t));
if(!pdat)
{
i2c_device_free(i2cdev);
return NULL;
}
c = malloc(sizeof(struct compass_t));
if(!c)
{
i2c_device_free(i2cdev);
free(pdat);
return NULL;
}
pdat->dev = i2cdev;
c->name = alloc_device_name(dt_read_name(n), -1);
c->ox = 0;
c->oy = 0;
c->oz = 0;
c->get = compass_hmc5883l_get;
c->priv = pdat;
if(!register_compass(&dev, c))
{
i2c_device_free(pdat->dev);
free_device_name(c->name);
free(c->priv);
free(c);
return NULL;
}
dev->driver = drv;
return dev;
}