void sc_lsm9ds0_shutdown(void)
{
uint8_t txbuf[2];
// Power down gyro
txbuf[0] = LSM9DS0_CTRL_REG1_G;
txbuf[1] = 0x00;
sc_i2c_write(i2cn_g, txbuf, sizeof(txbuf));
// Disable gyro data ready interrupt
txbuf[0] = LSM9DS0_CTRL_REG3_G;
txbuf[1] = 0x00; // I2_DRDY, gyroscope data ready
sc_i2c_write(i2cn_g, txbuf, sizeof(txbuf));
// Power down magn
txbuf[0] = LSM9DS0_CTRL_REG7_XM;
txbuf[1] = 0x02;
sc_i2c_write(i2cn_xm, txbuf, sizeof(txbuf));
// Disable magn data ready
txbuf[0] = LSM9DS0_CTRL_REG4_XM;
txbuf[1] = 0x00; // P2_DRDYM
sc_i2c_write(i2cn_xm, txbuf, sizeof(txbuf));
// Power down acc
txbuf[0] = LSM9DS0_CTRL_REG1_XM;
txbuf[1] = 0x00;
sc_i2c_write(i2cn_xm, txbuf, sizeof(txbuf));
// Disable acc data ready
txbuf[0] = LSM9DS0_CTRL_REG3_XM;
txbuf[1] = 0x00; // P1_DRDYA
sc_i2c_write(i2cn_xm, txbuf, sizeof(txbuf));
}
static void ms5611_reset(void)
{
uint8_t txbuf[1];
txbuf[0] = MS5611_RESET;
sc_i2c_write(i2cn, txbuf, sizeof(txbuf));
}
void sc_lsm9ds0_init(void)
{
uint8_t txbuf[2];
int8_t i2c_n;
sensors_ready = 0;
chMtxInit(&data_mtx);
chBSemInit(&lsm9ds0_drdy_sem, TRUE);
// Pinmux
palSetPadMode(SC_LSM9DS0_INT1_XM_PORT,
SC_LSM9DS0_INT1_XM_PIN,
PAL_MODE_INPUT);
palSetPadMode(SC_LSM9DS0_INT2_XM_PORT,
SC_LSM9DS0_INT2_XM_PIN,
PAL_MODE_INPUT);
palSetPadMode(SC_LSM9DS0_DRDY_G_PORT,
SC_LSM9DS0_DRDY_G_PIN,
PAL_MODE_INPUT);
palSetPadMode(SC_LSM9DS0_I2CN_SDA_PORT,
SC_LSM9DS0_I2CN_SDA_PIN,
PAL_MODE_ALTERNATE(SC_LSM9DS0_I2CN_SDA_AF));
palSetPadMode(SC_LSM9DS0_I2CN_SCL_PORT,
SC_LSM9DS0_I2CN_SCL_PIN,
PAL_MODE_ALTERNATE(SC_LSM9DS0_I2CN_SDA_AF));
// Register data ready interrupts
sc_extint_set_isr_cb(SC_LSM9DS0_INT1_XM_PORT,
SC_LSM9DS0_INT1_XM_PIN,
SC_EXTINT_EDGE_RISING,
lsm9ds0_drdy_cb);
sc_extint_set_isr_cb(SC_LSM9DS0_INT2_XM_PORT,
SC_LSM9DS0_INT2_XM_PIN,
SC_EXTINT_EDGE_RISING,
lsm9ds0_drdy_cb);
sc_extint_set_isr_cb(SC_LSM9DS0_DRDY_G_PORT,
SC_LSM9DS0_DRDY_G_PIN,
SC_EXTINT_EDGE_RISING,
lsm9ds0_drdy_cb);
// Initialize two I2C instances, one for acc+magn, one for gyro
i2c_n = sc_i2c_init(&SC_LSM9DS0_I2CN, SC_LSM9DS0_ADDR_XM);
i2cn_xm = (uint8_t)i2c_n;
i2c_n = sc_i2c_init(&SC_LSM9DS0_I2CN, SC_LSM9DS0_ADDR_G);
i2cn_g = (uint8_t)i2c_n;
// Setup and start acc+magn
// Shutdown the chip so that we get new data ready
// interrupts even if the chip was already running
sc_lsm9ds0_shutdown();
// Configure INT1_XM as acc data ready
txbuf[0] = LSM9DS0_CTRL_REG3_XM;
txbuf[1] = 0x04; // P1_DRDYA
sc_i2c_write(i2cn_xm, txbuf, sizeof(txbuf));
// Configure INT2_XM as magn data ready
txbuf[0] = LSM9DS0_CTRL_REG4_XM;
txbuf[1] = 0x04; // P2_DRDYM
sc_i2c_write(i2cn_xm, txbuf, sizeof(txbuf));
// Enable 100Hz acc
txbuf[0] = LSM9DS0_CTRL_REG1_XM;
txbuf[1] = 0x67; // XEN + YEN + ZEN + 100Hz
sc_i2c_write(i2cn_xm, txbuf, sizeof(txbuf));
// Configure 100Hz magn (and temperature)
txbuf[0] = LSM9DS0_CTRL_REG5_XM;
txbuf[1] = 0xF4; // High res, 100Hz, temp
sc_i2c_write(i2cn_xm, txbuf, sizeof(txbuf));
// Enable magn in continuous conversion mode
txbuf[0] = LSM9DS0_CTRL_REG7_XM;
txbuf[1] = 0x00; // Continuous conversion mode
sc_i2c_write(i2cn_xm, txbuf, sizeof(txbuf));
// Setup and start gyro
// Enable gyro data ready interrupt
txbuf[0] = LSM9DS0_CTRL_REG3_G;
txbuf[1] = 0x08; // I2_DRDY, gyroscope data ready
sc_i2c_write(i2cn_g, txbuf, sizeof(txbuf));
// Set gyro sensitivy to 500dps
txbuf[0] = LSM9DS0_CTRL_REG4_G;
txbuf[1] = 0x10; // FS0, 500 dps
sc_i2c_write(i2cn_g, txbuf, sizeof(txbuf));
// Enable gyroscope (95Hz with 25 cutoff (FIXME: what?))
txbuf[0] = LSM9DS0_CTRL_REG1_G;
txbuf[1] = 0x3F; // XEN + YEN + ZEN + PD + BW
sc_i2c_write(i2cn_g, txbuf, sizeof(txbuf));
}
