void LSM9DS0_readAccel(LSM9DS0_t* lsm_t) { uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp xmReadBytes(lsm_t,OUT_X_L_A, temp, 6); // Read 6 bytes, beginning at OUT_X_L_A lsm_t->ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax lsm_t->ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay lsm_t->az = (temp[5] << 8) | temp[4]; // Store z-axis values into az }
void LSM330D::readAccel() { uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp xmReadBytes(OUT_X_L_A, temp, 6); // Read 6 bytes, beginning at OUT_X_L_A ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay az = (temp[5] << 8) | temp[4]; // Store z-axis values into az }
void LSM9DS0_readMag(LSM9DS0_t* lsm_t) { uint8_t temp[6]; // We'll read six bytes from the mag into temp xmReadBytes(lsm_t,OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M lsm_t->mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx lsm_t->my = (temp[3] << 8) | temp[2]; // Store y-axis values into my lsm_t->mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz }
void LSM9DS0::readMag() { uint8_t temp[6]; // We'll read six bytes from the mag into temp xmReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx my = (temp[3] << 8) | temp[2]; // Store y-axis values into my mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz }
bool LSM9DS0::readAccel() { //uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp //xmReadBytes(OUT_X_L_A, temp, 6); // Read 6 bytes, beginning at OUT_X_L_A uint8_t temp[7]; // We'll read six bytes from the accelerometer into temp xmReadBytes(STATUS_REG_A, temp, 7); // Read 6 bytes, beginning at OUT_X_L_A if ( temp[0] & BIT_ZYXxDA ) { ax = (temp[2] << 8) | temp[1]; // Store x-axis values into ax ay = (temp[4] << 8) | temp[3]; // Store y-axis values into ay az = (temp[6] << 8) | temp[5]; // Store z-axis values into az return true; } return false; }
bool LSM9DS0::readMag() { //uint8_t temp[6]; // We'll read six bytes from the mag into temp uint8_t temp[7]; //xmReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M xmReadBytes(STATUS_REG_M, temp, 7); // Read 6 bytes, beginning at OUT_X_L_M if ( temp[0] & BIT_ZYXxDA) { mx = (temp[2] << 8) | temp[1]; // Store x-axis values into mx my = (temp[4] << 8) | temp[3]; // Store y-axis values into my mz = (temp[6] << 8) | temp[5]; // Store z-axis values into mz return true; } return false; }
void LSM330D::calLSM330D(float * gbias, float * abias) { uint8_t data[6] = {0, 0, 0, 0, 0, 0}; int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; uint16_t samples, ii; // First get gyro bias byte c = gReadByte(CTRL_REG5_G); gWriteByte(CTRL_REG5_G, c | 0x40); // Enable gyro FIFO delay(20); // Wait for change to take effect gWriteByte(FIFO_CTRL_REG_G, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples delay(1000); // delay 1000 milliseconds to collect FIFO samples samples = (gReadByte(FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples for(ii = 0; ii < samples ; ii++) { // Read the gyro data stored in the FIFO int16_t gyro_temp[3] = {0, 0, 0}; gReadBytes(OUT_X_L_G, &data[0], 6); gyro_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO gyro_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]); gyro_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]); gyro_bias[0] += (int32_t) gyro_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases gyro_bias[1] += (int32_t) gyro_temp[1]; gyro_bias[2] += (int32_t) gyro_temp[2]; } gyro_bias[0] /= samples; // average the data gyro_bias[1] /= samples; gyro_bias[2] /= samples; gbias[0] = (float)gyro_bias[0]*gRes; // Properly scale the data to get deg/s gbias[1] = (float)gyro_bias[1]*gRes; gbias[2] = (float)gyro_bias[2]*gRes; c = gReadByte(CTRL_REG5_G); gWriteByte(CTRL_REG5_G, c & ~0x40); // Disable gyro FIFO delay(20); gWriteByte(FIFO_CTRL_REG_G, 0x00); // Enable gyro bypass mode // Now get the accelerometer biases c = xmReadByte(CTRL_REG5_A); xmWriteByte(CTRL_REG5_A, c | 0x40); // Enable accelerometer FIFO delay(20); // Wait for change to take effect xmWriteByte(FIFO_CTRL_REG, 0x40 | 0x1F); // Enable accelerometer FIFO stream mode and set watermark at 32 samples delay(1000); // delay 1000 milliseconds to collect FIFO samples samples = (xmReadByte(FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples for(ii = 0; ii < samples ; ii++) { // Read the accelerometer data stored in the FIFO int16_t accel_temp[3] = {0, 0, 0}; xmReadBytes(OUT_X_L_A, &data[0], 6); accel_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]);// Form signed 16-bit integer for each sample in FIFO accel_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]); accel_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]); accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases accel_bias[1] += (int32_t) accel_temp[1]; accel_bias[2] += (int32_t) accel_temp[2]; } accel_bias[0] /= samples; // average the data accel_bias[1] /= samples; accel_bias[2] /= samples; if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) (1.0/aRes);} // Remove gravity from the z-axis accelerometer bias calculation else {accel_bias[2] += (int32_t) (1.0/aRes);} abias[0] = (float)accel_bias[0]*aRes; // Properly scale data to get gs abias[1] = (float)accel_bias[1]*aRes; abias[2] = (float)accel_bias[2]*aRes; c = xmReadByte(CTRL_REG5_A); xmWriteByte(CTRL_REG5_A, c & ~0x40); // Disable accelerometer FIFO delay(20); xmWriteByte(FIFO_CTRL_REG, 0x00); // Enable accelerometer bypass mode }
void LSM9DS0_readTemp(LSM9DS0_t* lsm_t) { uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp xmReadBytes(lsm_t,OUT_TEMP_L_XM, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L_M lsm_t->temperature = (((int16_t) temp[1] << 12) | temp[0] << 4 ) >> 4; // Temperature is a 12-bit signed integer }
void LSM9DS0_readTest(LSM9DS0_t* lsm_t) { uint8_t temp[2]; // We'll read six bytes from the mag into temp xmReadBytes(lsm_t,OUT_X_L_M, temp, 2); // Read 6 bytes, beginning at OUT_X_L_M lsm_t->mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx }
// This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average // them, scales them to gs and deg/s, respectively, and then passes the biases to the main sketch // for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store // the data as there are in the ADXL345, a precursor to the LSM9DS0, or the MPU-9150, so we have to // subtract the biases ourselves. This results in a more accurate measurement in general and can // remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner // is good practice. void calLSM9DS0(LSM9DS0_t* lsm_t, float * gbias, float * abias) { uint8_t data[6] = {0, 0, 0, 0, 0, 0}; int16_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; int samples, ii; // First get gyro bias uint8_t c = gReadByte(lsm_t,CTRL_REG5_G); //read modify write gWriteByte(lsm_t, CTRL_REG5_G, c | 0x40); // Enable gyro FIFO delay(20); // Wait for change to take effect gWriteByte(lsm_t, FIFO_CTRL_REG_G, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples delay(1000); // delay 1000 milliseconds to collect FIFO samples samples = (gReadByte(lsm_t,FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples for(ii = 0; ii < samples ; ii++) { // Read the gyro data stored in the FIFO gReadBytes(lsm_t,OUT_X_L_G, &data[0], 6); gyro_bias[0] += (((int16_t)data[1] << 8) | data[0]); gyro_bias[1] += (((int16_t)data[3] << 8) | data[2]); gyro_bias[2] += (((int16_t)data[5] << 8) | data[4]); } gyro_bias[0] /= samples; // average the data gyro_bias[1] /= samples; gyro_bias[2] /= samples; gbias[0] = (float)gyro_bias[0]*lsm_t->gRes; // Properly scale the data to get deg/s gbias[1] = (float)gyro_bias[1]*lsm_t->gRes; gbias[2] = (float)gyro_bias[2]*lsm_t->gRes; c = gReadByte(lsm_t,CTRL_REG5_G); gWriteByte(lsm_t, CTRL_REG5_G, c & ~0x40); // Disable gyro FIFO delay(20); gWriteByte(lsm_t, FIFO_CTRL_REG_G, 0x00); // Enable gyro bypass mode // Now get the accelerometer biases c = xmReadByte(lsm_t,CTRL_REG0_XM); xmWriteByte(lsm_t,CTRL_REG0_XM, c | 0x40); // Enable accelerometer FIFO delay(20); // Wait for change to take effect xmWriteByte(lsm_t,FIFO_CTRL_REG, 0x20 | 0x1F); // Enable accelerometer FIFO stream mode and set watermark at 32 samples delay(1000); // delay 1000 milliseconds to collect FIFO samples samples = (xmReadByte(lsm_t,FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples for(ii = 0; ii < samples ; ii++) { // Read the accelerometer data stored in the FIFO xmReadBytes(lsm_t,OUT_X_L_A, &data[0], 6); accel_bias[0] += (((int16_t)data[1] << 8) | data[0]); accel_bias[1] += (((int16_t)data[3] << 8) | data[2]); accel_bias[2] += (((int16_t)data[5] << 8) | data[4]) - (int16_t)(1.0f/lsm_t->aRes); // Assumes sensor facing up! } accel_bias[0] /= samples; // average the data accel_bias[1] /= samples; accel_bias[2] /= samples; abias[0] = (float)accel_bias[0]*lsm_t->aRes; // Properly scale data to get gs abias[1] = (float)accel_bias[1]*lsm_t->aRes; abias[2] = (float)accel_bias[2]*lsm_t->aRes; c = xmReadByte(lsm_t,CTRL_REG0_XM); xmWriteByte(lsm_t,CTRL_REG0_XM, c & ~0x40); // Disable accelerometer FIFO delay(20); xmWriteByte(lsm_t,FIFO_CTRL_REG, 0x00); // Enable accelerometer bypass mode }
void LSM9DS0::readTemp() { uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp xmReadBytes(OUT_TEMP_L_XM, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L_M temperature = int16_t(temp[0]) + (int16_t(temp[1])<<8) ; // Temperature is a 12-bit signed integer }