void AP_InertialSensor_LSM303D::disable_i2c(void) { uint8_t a = _register_read(0x02); _register_write(0x02, (0x10 | a)); a = _register_read(0x02); _register_write(0x02, (0xF7 & a)); a = _register_read(0x15); _register_write(0x15, (0x80 | a)); a = _register_read(0x02); _register_write(0x02, (0xE7 & a)); }
uint8_t AP_InertialSensor_L3GD20::set_range(uint8_t max_dps) { uint8_t bits = REG4_BDU; float new_range_scale_dps_digit; float new_range; if (max_dps == 0) { max_dps = 2000; } if (max_dps <= 250) { new_range = 250; bits |= RANGE_250DPS; new_range_scale_dps_digit = 8.75e-3f; } else if (max_dps <= 500) { new_range = 500; bits |= RANGE_500DPS; new_range_scale_dps_digit = 17.5e-3f; } else if (max_dps <= 2000) { new_range = 2000; bits |= RANGE_2000DPS; new_range_scale_dps_digit = 70e-3f; } else { return -1; } // _gyro_range_rad_s = new_range / 180.0f * M_PI_F; // _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; _gyro_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; _register_write(ADDR_CTRL_REG4, bits); return 0; }
uint8_t AP_InertialSensor_L3GD20::set_samplerate(uint16_t frequency) { uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; if (frequency == 0) frequency = 760; /* use limits good for H or non-H models */ if (frequency <= 100) { // _current_rate = 95; bits |= RATE_95HZ_LP_25HZ; } else if (frequency <= 200) { // _current_rate = 190; bits |= RATE_190HZ_LP_50HZ; } else if (frequency <= 400) { // _current_rate = 380; bits |= RATE_380HZ_LP_50HZ; } else if (frequency <= 800) { // _current_rate = 760; bits |= RATE_760HZ_LP_50HZ; } else { return -1; } _register_write(ADDR_CTRL_REG1, bits); return 0; }
void AP_InertialSensor_LSM303D::_register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits) { uint8_t val; val = _register_read(reg); val &= ~clearbits; val |= setbits; _register_write(reg, val); }
/* useful when debugging SPI bus errors */ void AP_InertialSensor_L3GD20::_register_write_check(uint8_t reg, uint8_t val) { uint8_t readed; _register_write(reg, val); readed = _register_read(reg); if (readed != val){ hal.console->printf_P(PSTR("Values doesn't match; written: %02x; read: %02x "), val, readed); } #if L3GD20_DEBUG hal.console->printf_P(PSTR("Values written: %02x; readed: %02x "), val, readed); #endif }
void AP_InertialSensor_L3GD20::disable_i2c(void) { uint8_t retries = 10; while (retries--) { // add retries uint8_t a = _register_read(0x05); _register_write(0x05, (0x20 | a)); if (_register_read(0x05) == (a | 0x20)) { return; } } hal.scheduler->panic(PSTR("L3GD20: Unable to disable I2C")); }
bool AP_Compass_AK8963::_calibrate() { error("CALIBRATTION START\n"); _register_write(AK8963_CNTL1, AK8963_FUSE_MODE | _magnetometer_adc_resolution); /* Enable FUSE-mode in order to be able to read calibreation data */ hal.scheduler->delay(10); uint8_t response[3]; _register_read(AK8963_ASAX, 0x03, response); for (int i = 0; i < 3; i++) { float data = response[i]; magnetometer_ASA[i] = ((data-128)/256+1); error("%d: %lf\n", i, magnetometer_ASA[i]); } error("CALIBRATTION END\n"); return true; }
bool AP_Compass_AK8963::init() { _healthy[0] = true; _field[0].x = 0.0f; _field[0].y = 0.0f; _field[0].z = 0.0f; hal.scheduler->suspend_timer_procs(); if (!_backend->sem_take_blocking()) { error("_spi_sem->take failed\n"); return false; } if (!_backend_init()) { _backend->sem_give(); return false; } _register_write(AK8963_CNTL2, AK8963_RESET); /* Reset AK8963 */ hal.scheduler->delay(1000); int id_mismatch_count; uint8_t deviceid; for (id_mismatch_count = 0; id_mismatch_count < 5; id_mismatch_count++) { _register_read(AK8963_WIA, 0x01, &deviceid); /* Read AK8963's id */ if (deviceid == AK8963_Device_ID) { break; } error("trying to read AK8963's ID once more...\n"); _backend_reset(); hal.scheduler->delay(100); _dump_registers(); } if (id_mismatch_count == 5) { _initialised = false; hal.console->printf("WRONG AK8963 DEVICE ID: 0x%x\n", (unsigned)deviceid); hal.scheduler->panic(PSTR("AK8963: bad DEVICE ID")); } _calibrate(); _initialised = true; #if AK8963_SELFTEST if (_self_test()) { _initialised = true; } else { _initialised = false; } #endif /* Register value to continuous measurement */ _register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | _magnetometer_adc_resolution); _backend->sem_give(); hal.scheduler->resume_timer_procs(); hal.scheduler->register_timer_process( AP_HAL_MEMBERPROC(&AP_Compass_AK8963::_update)); _start_conversion(); _initialised = true; return _initialised; }
bool AP_Compass_AK8963::_self_test() { bool success = false; /* see AK8963.pdf p.19 */ /* Set power-down mode */ _register_write(AK8963_CNTL1, AK8963_POWERDOWN_MODE | _magnetometer_adc_resolution); /* Turn the internal magnetic field on */ _register_write(AK8963_ASTC, AK8983_SELFTEST_MAGNETIC_FIELD_ON); /* Register value to self-test mode in 14-bit */ _register_write(AK8963_CNTL1, AK8963_SELFTEST_MODE | _magnetometer_adc_resolution); _start_conversion(); hal.scheduler->delay(20); _read_raw(); float hx = _mag_x; float hy = _mag_y; float hz = _mag_z; error("AK8963's SELF-TEST STARTED\n"); switch (_magnetometer_adc_resolution) { bool hx_is_in_range; bool hy_is_in_range; bool hz_is_in_range; case AK8963_14BIT_ADC: hx_is_in_range = (hx >= - 50) && (hx <= 50); hy_is_in_range = (hy >= - 50) && (hy <= 50); hz_is_in_range = (hz >= - 800) && (hz <= -200); if (hx_is_in_range && hy_is_in_range && hz_is_in_range) { success = true; } break; case AK8963_16BIT_ADC: hx_is_in_range = (hx >= -200) && (hx <= 200); hy_is_in_range = (hy >= -200) && (hy <= 200); hz_is_in_range = (hz >= -3200) && (hz <= -800); if (hx_is_in_range && hy_is_in_range && hz_is_in_range) { success = true; } break; default: success = false; hal.scheduler->panic(PSTR("Wrong AK8963's ADC resolution selected")); break; } error("AK8963's SELF-TEST ENDED: %f %f %f\n", hx, hy, hz); /* Turn the internal magnetic field off */ _register_write(AK8963_ASTC, 0x0); /* Register value to continuous measurement in 14-bit */ _register_write(AK8963_CNTL1, AK8963_POWERDOWN_MODE | _magnetometer_adc_resolution); return success; }
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; }