void
L3GD20::reset()
{
// ensure the chip doesn't interpret any other bus traffic as I2C
disable_i2c();
/* set default configuration */
write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
write_reg(ADDR_CTRL_REG4, REG4_BDU);
write_reg(ADDR_CTRL_REG5, 0);
write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
/* disable FIFO. This makes things simpler and ensures we
* aren't getting stale data. It means we must run the hrt
* callback fast enough to not miss data. */
write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
set_samplerate(0); // 760Hz
set_range(L3GD20_DEFAULT_RANGE_DPS);
set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
_read = 0;
}
void
LSM303D::reset()
{
// ensure the chip doesn't interpret any other bus traffic as I2C
disable_i2c();
/* enable accel*/
_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
write_reg(ADDR_CTRL_REG1, _reg1_expected);
/* enable mag */
_reg7_expected = REG7_CONT_MODE_M;
write_reg(ADDR_CTRL_REG7, _reg7_expected);
write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
// we setup the anti-alias on-chip filter as 50Hz. We believe
// this operates in the analog domain, and is critical for
// anti-aliasing. The 2 pole software filter is designed to
// operate in conjunction with this on-chip filter
accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
_accel_read = 0;
_mag_read = 0;
}
void
main(void)
{
twi_master_init();
mpu6500_init();
mpu6500_stop();
disable_i2c();
simble_init("Motion");
motion_ctx.sampling_period = DEFAULT_SAMPLING_PERIOD;
//Set the timer parameters and initialize it.
struct rtc_ctx rtc_ctx = {
.rtc_x[NOTIF_TIMER_ID] = {
.type = PERIODIC,
.period = motion_ctx.sampling_period,
.enabled = false,
.cb = notif_timer_cb,
}
};
batt_serv_init(&rtc_ctx);
rtc_init(&rtc_ctx);
ind_init();
motion_init(&motion_ctx);
simble_adv_start();
simble_process_event_loop();
}
static void
motion_read(struct service_desc *s, struct char_desc *c, void **valp, uint16_t *lenp)
{
struct motion_ctx *ctx = (void *)s;
enable_i2c();
mpu6500_start();
nrf_delay_us(MPU6500_WAKEUP_TIME);
mpu6500_read_data(&ctx->motion_value);
mpu6500_stop();
disable_i2c();
*lenp = sizeof(ctx->motion_value);
*valp = &ctx->motion_value;
}
bool AP_InertialSensor_L3GD20::_hardware_init(Sample_rate sample_rate)
{
if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("L3GD20: Unable to get semaphore"));
}
// initially run the bus at low speed
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
// ensure the chip doesn't interpret any other bus traffic as I2C
disable_i2c();
// Chip reset
/* set default configuration */
_register_write(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
hal.scheduler->delay(1);
_register_write(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
hal.scheduler->delay(1);
_register_write(ADDR_CTRL_REG3, 0x08); /* DRDY enable */
hal.scheduler->delay(1);
_register_write(ADDR_CTRL_REG4, REG4_BDU);
hal.scheduler->delay(1);
_register_write(ADDR_CTRL_REG5, 0);
hal.scheduler->delay(1);
_register_write(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
hal.scheduler->delay(1);
/* disable FIFO. This makes things simpler and ensures we
* aren't getting stale data. It means we must run the hrt
* callback fast enough to not miss data. */
_register_write(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
hal.scheduler->delay(1);
set_samplerate(0); // 760Hz
hal.scheduler->delay(1);
set_range(L3GD20_DEFAULT_RANGE_DPS);
hal.scheduler->delay(1);
// //TODO: Filtering
// uint8_t default_filter;
// // sample rate and filtering
// // to minimise the effects of aliasing we choose a filter
// // that is less than half of the sample rate
// switch (sample_rate) {
// case RATE_50HZ:
// // this is used for plane and rover, where noise resistance is
// // more important than update rate. Tests on an aerobatic plane
// // show that 10Hz is fine, and makes it very noise resistant
// default_filter = BITS_DLPF_CFG_10HZ;
// _sample_shift = 2;
// break;
// case RATE_100HZ:
// default_filter = BITS_DLPF_CFG_20HZ;
// _sample_shift = 1;
// break;
// case RATE_200HZ:
// default:
// default_filter = BITS_DLPF_CFG_20HZ;
// _sample_shift = 0;
// break;
// }
// _set_filter_register(_L3GD20_filter, default_filter);
// now that we have initialised, we set the SPI bus speed to high
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_spi_sem->give();
return true;
}
bool AP_InertialSensor_LSM303D::_hardware_init(Sample_rate sample_rate)
{
if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("LSM303D: Unable to get semaphore"));
}
// initially run the bus at low speed
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
// ensure the chip doesn't interpret any other bus traffic as I2C
disable_i2c();
/* enable accel*/
_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
_register_write(ADDR_CTRL_REG1, _reg1_expected);
/* enable mag */
_reg7_expected = REG7_CONT_MODE_M;
_register_write(ADDR_CTRL_REG7, _reg7_expected);
_register_write(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
_register_write(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
_register_write(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
// Hardware filtering
// we setup the anti-alias on-chip filter as 50Hz. We believe
// this operates in the analog domain, and is critical for
// anti-aliasing. The 2 pole software filter is designed to
// operate in conjunction with this on-chip filter
accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
// TODO: Software filtering
// accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
// uint8_t default_filter;
// // sample rate and filtering
// // to minimise the effects of aliasing we choose a filter
// // that is less than half of the sample rate
// switch (sample_rate) {
// case RATE_50HZ:
// // this is used for plane and rover, where noise resistance is
// // more important than update rate. Tests on an aerobatic plane
// // show that 10Hz is fine, and makes it very noise resistant
// default_filter = BITS_DLPF_CFG_10HZ;
// _sample_shift = 2;
// break;
// case RATE_100HZ:
// default_filter = BITS_DLPF_CFG_20HZ;
// _sample_shift = 1;
// break;
// case RATE_200HZ:
// default:
// default_filter = BITS_DLPF_CFG_20HZ;
// _sample_shift = 0;
// break;
// }
// _set_filter_register(_LSM303D_filter, default_filter);
// now that we have initialised, we set the SPI bus speed to high
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_spi_sem->give();
return true;
}
