Beispiel #1
0
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;
}
Beispiel #2
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;
}