int BMI160::reset() { write_reg(BMIREG_CONF, (1 << 1)); //Enable NVM programming write_checked_reg(BMIREG_ACC_CONF, BMI_ACCEL_US | BMI_ACCEL_BWP_NORMAL); //Normal operation, no decimation write_checked_reg(BMIREG_ACC_RANGE, 0); write_checked_reg(BMIREG_GYR_CONF, BMI_GYRO_BWP_NORMAL); //Normal operation, no decimation write_checked_reg(BMIREG_GYR_RANGE, 0); write_checked_reg(BMIREG_INT_EN_1, BMI_DRDY_INT_EN); //Enable DRDY interrupt write_checked_reg(BMIREG_INT_OUT_CTRL, BMI_INT1_EN); //Enable interrupts on pin INT1 write_checked_reg(BMIREG_INT_MAP_1, BMI_DRDY_INT1); //DRDY interrupt on pin INT1 write_checked_reg(BMIREG_IF_CONF, BMI_SPI_4_WIRE | BMI_AUTO_DIS_SEC); //Disable secondary interface; Work in SPI 4-wire mode write_checked_reg(BMIREG_NV_CONF, BMI_SPI); //Disable I2C interface set_accel_range(BMI160_ACCEL_DEFAULT_RANGE_G); accel_set_sample_rate(BMI160_ACCEL_DEFAULT_RATE); set_gyro_range(BMI160_GYRO_DEFAULT_RANGE_DPS); gyro_set_sample_rate(BMI160_GYRO_DEFAULT_RATE); //_set_dlpf_filter(BMI160_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); //NOT CONSIDERING FILTERING YET //Enable Accelerometer in normal mode write_reg(BMIREG_CMD, BMI_ACCEL_NORMAL_MODE); up_udelay(4100); //usleep(4100); //Enable Gyroscope in normal mode write_reg(BMIREG_CMD, BMI_GYRO_NORMAL_MODE); up_udelay(80300); //usleep(80300); uint8_t retries = 10; while (retries--) { bool all_ok = true; for (uint8_t i = 0; i < BMI160_NUM_CHECKED_REGISTERS; i++) { if (read_reg(_checked_registers[i]) != _checked_values[i]) { write_reg(_checked_registers[i], _checked_values[i]); all_ok = false; } } if (all_ok) { break; } } _accel_reads = 0; _gyro_reads = 0; return OK; }
int BMI160::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_gyro_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case GYROIOCGSAMPLERATE: return _gyro_sample_rate; case GYROIOCSSAMPLERATE: return gyro_set_sample_rate(arg); case GYROIOCSSCALE: /* copy scale in */ memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: return set_gyro_range(arg); case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); } }
int BMI055_gyro::reset() { write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset usleep(5000); write_checked_reg(BMI055_GYR_BW, 0); // Write Gyro Bandwidth write_checked_reg(BMI055_GYR_RANGE, 0);// Write Gyro range write_checked_reg(BMI055_GYR_INT_EN_0, BMI055_GYR_DRDY_INT_EN); //Enable DRDY interrupt write_checked_reg(BMI055_GYR_INT_MAP_1, BMI055_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1 set_gyro_range(BMI055_GYRO_DEFAULT_RANGE_DPS);// set Gyro range gyro_set_sample_rate(BMI055_GYRO_DEFAULT_RATE);// set Gyro ODR //Enable Gyroscope in normal mode write_reg(BMI055_GYR_LPM1, BMI055_GYRO_NORMAL); up_udelay(1000); uint8_t retries = 10; while (retries--) { bool all_ok = true; for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) { if (read_reg(_checked_registers[i]) != _checked_values[i]) { write_reg(_checked_registers[i], _checked_values[i]); all_ok = false; } } if (all_ok) { break; } } _gyro_reads = 0; return OK; }
int BMI160::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_gyro_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); case GYROIOCGSAMPLERATE: return _gyro_sample_rate; case GYROIOCSSAMPLERATE: return gyro_set_sample_rate(arg); case GYROIOCGLOWPASS: return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSLOWPASS: // set software filtering _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case GYROIOCSSCALE: /* copy scale in */ memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: return set_gyro_range(arg); case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); case GYROIOCSELFTEST: return gyro_self_test(); #ifdef GYROIOCSHWLOWPASS case GYROIOCSHWLOWPASS: _set_dlpf_filter(arg); return OK; #endif #ifdef GYROIOCGHWLOWPASS case GYROIOCGHWLOWPASS: return _dlpf_freq; #endif default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); } }
int BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ case 0: return -EINVAL; /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, BMI055_GYRO_MAX_RATE); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, BMI055_GYRO_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; /* check against maximum rate */ if (ticks < 1000) { return -EINVAL; } float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); float sample_rate = 1.0e6f / ticks; _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); /* update interval for next measurement */ _call_interval = ticks; /* set call interval faster than the sample time. We then detect when we have duplicate samples and reject them. This prevents aliasing due to a beat between the stm32 clock and the bmi055 clock */ _call.period = _call_interval - BMI055_TIMER_REDUCTION; /* if we need to start the poll state machine, do it */ if (want_start) { start(); } return OK; } } } case SENSORIOCGPOLLRATE: if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; } return 1000000 / _call_interval; case SENSORIOCRESET: return reset(); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { return -EINVAL; } irqstate_t flags = px4_enter_critical_section(); if (!_gyro_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case GYROIOCGSAMPLERATE: return _gyro_sample_rate; case GYROIOCSSAMPLERATE: return gyro_set_sample_rate(arg); case GYROIOCSSCALE: /* copy scale in */ memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: return set_gyro_range(arg); case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); case GYROIOCSELFTEST: return gyro_self_test(); default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); } }