Hello,
I need some help in figuring out why my L3GD20H gyro is giving me strange values when using auto increment. The sensor is embedded into the AltIMU-10 v4,
I am using a Raspberry Pi Zero to talk over i2c with the IMU. The auto increment work just fine for the LPS25H and LSM303D. Below is my header and source file for the L3GD20H
L3GD20H.cpp
// L3GD20H.cpp: L3GD20H, gyro sensor library
// Includes
#include "L3GD20H.h"
// Initialize object
L3GD20H::L3GD20H(uint8 address) {
_address = address;
_status = (uint8) SENSOR_NOT_INIT;
int8 fileID;
char const *device = "/dev/i2c-1";
if (((fileID = open(device, O_RDWR)) < 0) || (ioctl(fileID, I2C_SLAVE, _address) < 0)) {
printf("L3GD20H could not be initialize\n");
_status = (uint8) SENSOR_ERROR_INIT;
} else {
printf("L3GD20H initialized at %i\n", address);
_status = (uint8) SENSOR_OK;
_fileID = fileID;
// Configure sensor
int8 didWrite = 0;
// Enable XYZ gyro axis at 200 Hz, normal mode
didWrite |= i2c_smbus_write_byte_data(_fileID, CTRL1, 0b01011111);
// Continues data with scaling 500 deg/s
didWrite |= i2c_smbus_write_byte_data(_fileID, CTRL4, 0b00010000);
// Set error code if something went wrong
if (didWrite < 0) {
_status |= (uint8) SENSOR_ERROR_CONF;
}
}
}
// Read sensor data to buffer
void L3GD20H::read() {
// Array with registers to read
uint8 regLen = 6;
uint8 reg[regLen] = {OUT_X_L, OUT_X_H, OUT_Y_L, OUT_Y_H,
OUT_Z_L, OUT_Z_H};
// Group registers for auto incrementation
uint8 groupRegLen = 1;
uint8 groupReg[groupRegLen] = {6};
// Read data
int16 readByte;
int8 didRead = 0;
uint8 regi = 0;
for (uint8 regGroupi = 0; regGroupi < groupRegLen; regGroupi++) {
// Read first byte using register address explicitly, set auto increment bit
readByte = i2c_smbus_read_byte_data(_fileID, (reg[regi] | 0b10000000));
_buffer[reg[regi]] = (uint8) readByte;
didRead |= (int8) (readByte >> 8);
regi ++;
// Read remaining bytes (if any) implicitly by auto incrementing the register
for (uint8 groupi = 1; groupi < groupReg[regGroupi]; groupi++) {
readByte = i2c_smbus_read_byte(_fileID);
_buffer[reg[regi]] = (uint8) readByte;
didRead |= (int8) (readByte >> 8);
regi ++;
}
}
// Set error code if something went wrong
if (didRead < 0) {
_status |= (uint8) SENSOR_ERROR_READ;
} else {
_status &= (uint8) ~SENSOR_ERROR_READ;
}
}
// Returns angular rate about x [deg/s]
float L3GD20H::getAngularRateX() {
int16 rawAngRateX = (_buffer[OUT_X_H] << 8) | _buffer[OUT_X_L];
float angRateX = float(rawAngRateX)*float(GYRO_SCALE)/float(GYRO_RESOLUTION);
return angRateX;
}
// Returns angular rate about y [deg/s]
float L3GD20H::getAngularRateY() {
int16 rawAngRateY = (_buffer[OUT_Y_H] << 8) | _buffer[OUT_Y_L];
float angRateY = float(rawAngRateY)*float(GYRO_SCALE)/float(GYRO_RESOLUTION);
return angRateY;
}
// Returns angular rate about z [deg/s]
float L3GD20H::getAngularRateZ() {
int16 rawAngRateZ = (_buffer[OUT_Z_H] << 8) | _buffer[OUT_Z_L];
float angRateZ = float(rawAngRateZ)*float(GYRO_SCALE)/float(GYRO_RESOLUTION);
return angRateZ;
}
// Returns sensor status
uint8 L3GD20H::getStatus() {
return _status;
}
L3GD20H.h
/* L3GD20H.h: L3GD20H, gyro sensor library
------------------------------------------------
Title: L3GD20H sensor library
For data sheet see, https://www.pololu.com/file/download/L3GD20H.pdf?file_id=0J731
*/
#ifndef L3GD20H_H
#define L3GD20H_H
// Includes
#include <stdio.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/i2c-dev.h>
#include "GenFunc.h"
#include "DataTypes.h"
// Register addresses
#define WHO_AM_I 0x0F
#define CTRL1 0x20
#define CTRL2 0x21
#define CTRL3 0x22
#define CTRL4 0x23
#define CTRL5 0x24
#define REFERENCE 0x25
#define OUT_TEMP 0x26
#define STATUS 0x27
#define OUT_X_L 0x28
#define OUT_X_H 0x29
#define OUT_Y_L 0x2A
#define OUT_Y_H 0x2B
#define OUT_Z_L 0x2C
#define OUT_Z_H 0x2D
#define FIFO_CTRL 0x2E
#define FIFO_SRC 0x2F
#define IG_CFG 0x30
#define IG_SRC 0x31
#define IG_THS_XH 0x32
#define IG_THS_XL 0x33
#define IG_THS_YH 0x34
#define IG_THS_YL 0x35
#define IG_THS_ZH 0x36
#define IG_THS_ZL 0x37
#define IG_DURATION 0x38
#define LOW_ODR 0x39
// Sensor scale
#define GYRO_SCALE 500 // [deg/s]
// Sensor resolution
#define GYRO_RESOLUTION 32767 // 2^15-1 (16-bit signed integer)
// Bit mask for sensor status
#define SENSOR_OK 0
#define SENSOR_NOT_INIT 1
#define SENSOR_ERROR_INIT 2
#define SENSOR_ERROR_CONF 4
#define SENSOR_ERROR_READ 8
// L3GD20H class
class L3GD20H {
public:
// Constructor for L3GD20H instance. Requires input of I2C address
L3GD20H(uint8 address);
// Read sensor data to buffer
void read();
// Returns angular rate about x [deg/s]
float getAngularRateX();
// Returns angular rate about y [deg/s]
float getAngularRateY();
// Returns angular rate about z [deg/s]
float getAngularRateZ();
// Returns sensor status
uint8 getStatus();
private:
// I2C address & file ID
uint8 _address;
int8 _fileID;
// Buffer to store data
uint8 _buffer[64];
// Sensor status
uint8 _status;
};
#endif
When I am not using auto increment it work fine. Below are two figures without/with auto increment. Note the strange values for the gyro (offset and very noisy).
Without Auto Increment
With Auto Increment
I need your support. I have read though the data sheet but I can’t find the reason for this. What am I missing?
Thanks in advance!
Update
Below is the source code for L3GD20H without using auto increment and the source code for another sensor, LSM303D, using auto increment.
L3GD20H.cpp without auto increment
// L3GD20H.cpp: L3GD20H, gyro sensor library
// Includes
#include "L3GD20H.h"
// Initialize object
L3GD20H::L3GD20H(uint8 address) {
_address = address;
_status = (uint8) SENSOR_NOT_INIT;
int8 fileID;
char const *device = "/dev/i2c-1";
if (((fileID = open(device, O_RDWR)) < 0) || (ioctl(fileID, I2C_SLAVE, _address) < 0)) {
printf("L3GD20H could not be initialize\n");
_status = (uint8) SENSOR_ERROR_INIT;
} else {
printf("L3GD20H initialized at %i\n", address);
_status = (uint8) SENSOR_OK;
_fileID = fileID;
// Configure sensor
int8 didWrite = 0;
// Enable XYZ gyro axis at 200 Hz, normal mode
didWrite |= i2c_smbus_write_byte_data(_fileID, CTRL1, 0b01011111);
// Continues data with scaling 500 deg/s
didWrite |= i2c_smbus_write_byte_data(_fileID, CTRL4, 0b00010000);
// Set error code if something went wrong
if (didWrite < 0) {
_status |= (uint8) SENSOR_ERROR_CONF;
}
}
}
// Read sensor data to buffer
void L3GD20H::read() {
// Array with registers to read
uint8 regLen = 6;
uint8 reg[regLen] = {OUT_X_L, OUT_X_H, OUT_Y_L, OUT_Y_H,
OUT_Z_L, OUT_Z_H};
// Read from sensor
int16 readByte;
int8 didRead = 0;
for (uint8 regi = 0; regi < regLen; regi++) {
readByte = i2c_smbus_read_byte_data(_fileID, reg[regi]);
_buffer[reg[regi]] = (uint8) readByte;
didRead |= (int8) (readByte >> 8);
}
// Set error code if something went wrong
if (didRead < 0) {
_status |= (uint8) SENSOR_ERROR_READ;
} else {
_status &= (uint8) ~SENSOR_ERROR_READ;
}
}
// Returns angular rate about x [deg/s]
float L3GD20H::getAngularRateX() {
int16 rawAngRateX = (_buffer[OUT_X_H] << 8) | _buffer[OUT_X_L];
float angRateX = float(rawAngRateX)*float(GYRO_SCALE)/float(GYRO_RESOLUTION);
return angRateX;
}
// Returns angular rate about y [deg/s]
float L3GD20H::getAngularRateY() {
int16 rawAngRateY = (_buffer[OUT_Y_H] << 8) | _buffer[OUT_Y_L];
float angRateY = float(rawAngRateY)*float(GYRO_SCALE)/float(GYRO_RESOLUTION);
return angRateY;
}
// Returns angular rate about z [deg/s]
float L3GD20H::getAngularRateZ() {
int16 rawAngRateZ = (_buffer[OUT_Z_H] << 8) | _buffer[OUT_Z_L];
float angRateZ = float(rawAngRateZ)*float(GYRO_SCALE)/float(GYRO_RESOLUTION);
return angRateZ;
}
// Returns sensor status
uint8 L3GD20H::getStatus() {
return _status;
}
LSM303D.cpp with auto increment
// LSM303D.cpp: LSM303D, accelerometer and magnetometer sensor library
// Includes
#include "LSM303D.h"
// Initialize object
LSM303D::LSM303D(uint8 address) {
_address = address;
_status = (uint8) SENSOR_NOT_INIT;
int8 fileID;
char const *device = "/dev/i2c-1";
if (((fileID = open(device, O_RDWR)) < 0) || (ioctl(fileID, I2C_SLAVE, _address) < 0)) {
printf("LSM303D could not be initialize\n");
_status = (uint8) SENSOR_ERROR_INIT;
} else {
printf("LSM303D initialized at %i\n", address);
_status = (uint8) SENSOR_OK;
_fileID = fileID;
// Configure sensor
int8 didWrite = 0;
// Enable XYZ accelerometer axis for continues output at 200 Hz
didWrite |= i2c_smbus_write_byte_data(_fileID, CTRL1, 0b01110111);
// Accelerometer scale selection, ± 4 g
didWrite |= i2c_smbus_write_byte_data(_fileID, CTRL2, 0b00001000);
/* Enable and temperature sensor at 6.25 Hz (also magnetometer rate),
high resolution */
didWrite |= i2c_smbus_write_byte_data(_fileID, CTRL5, 0b11100100);
// Magnetometer scale selection, ± 4 gauss
didWrite |= i2c_smbus_write_byte_data(_fileID, CTRL6, 0b00100000);
// Magnetometer normal, continuous-conversion mode
didWrite |= i2c_smbus_write_byte_data(_fileID, CTRL7, 0b00000000);
// Set error code if something went wrong
if (didWrite < 0) {
_status |= (uint8) SENSOR_ERROR_CONF;
}
}
}
// Read sensor data to buffer
void LSM303D::read() {
// Array with registers to read
uint8 regLen = 12;
uint8 reg[regLen] = {OUT_X_L_A, OUT_X_H_A, OUT_Y_L_A, OUT_Y_H_A,
OUT_Z_L_A, OUT_Z_H_A, OUT_X_L_M, OUT_X_H_M,
OUT_Y_L_M, OUT_Y_H_M, OUT_Z_L_M, OUT_Z_H_M};
// Group registers for auto incrementation
uint8 groupRegLen = 2;
uint8 groupReg[groupRegLen] = {6 6};
// Read data
int16 readByte;
int8 didRead = 0;
uint8 regi = 0;
for (uint8 regGroupi = 0; regGroupi < groupRegLen; regGroupi++) {
// Read first byte using register address explicitly, set auto increment bit
readByte = i2c_smbus_read_byte_data(_fileID, (reg[regi] | 0b10000000));
_buffer[reg[regi]] = (uint8) readByte;
didRead |= (int8) (readByte >> 8);
regi ++;
// Read remaining bytes (if any) implicitly by auto incrementing the register
for (uint8 groupi = 1; groupi < groupReg[regGroupi]; groupi++) {
readByte = i2c_smbus_read_byte(_fileID);
_buffer[reg[regi]] = (uint8) readByte;
didRead |= (int8) (readByte >> 8);
regi ++;
}
}
// Set error code if something went wrong
if (didRead < 0) {
_status |= (uint8) SENSOR_ERROR_READ;
} else {
_status &= (uint8) ~SENSOR_ERROR_READ;
}
}
// Returns acceleration component in x [g]
float LSM303D::getAccelerationX() {
int16 rawAccX = (_buffer[OUT_X_H_A] << 8) | _buffer[OUT_X_L_A];
float accX = float(rawAccX)*float(ACC_SCALE)/float(ACC_RESOLUTION);
return accX;
}
// Returns acceleration component in y [g]
float LSM303D::getAccelerationY() {
int16 rawAccY = (_buffer[OUT_Y_H_A] << 8) | _buffer[OUT_Y_L_A];
float accY = float(rawAccY)*float(ACC_SCALE)/float(ACC_RESOLUTION);
return accY;
}
// Returns acceleration component in z [g]
float LSM303D::getAccelerationZ() {
int16 rawAccZ = (_buffer[OUT_Z_H_A] << 8) | _buffer[OUT_Z_L_A];
float accZ = float(rawAccZ)*float(ACC_SCALE)/float(ACC_RESOLUTION);
return accZ;
}
// Returns magnetic field component in x [gauss]
float LSM303D::getMagneticFieldX() {
int16 rawMagX = (_buffer[OUT_X_H_M] << 8) | _buffer[OUT_X_L_M];
float magX = float(rawMagX)*float(MAG_SCALE)/float(MAG_RESOLUTION);
return magX;
}
// Returns magnetic field component in y [gauss]
float LSM303D::getMagneticFieldY() {
int16 rawMagY = (_buffer[OUT_Y_H_M] << 8) | _buffer[OUT_Y_L_M];
float magY = float(rawMagY)*float(MAG_SCALE)/float(MAG_RESOLUTION);
return magY;
}
// Returns magnetic field component in z [gauss]
float LSM303D::getMagneticFieldZ() {
int16 rawMagZ = (_buffer[OUT_Z_H_M] << 8) | _buffer[OUT_Z_L_M];
float magZ = float(rawMagZ)*float(MAG_SCALE)/float(MAG_RESOLUTION);
return magZ;
}
// Returns sensor status
uint8 LSM303D::getStatus() {
return _status;
}