ADXL345 Accelerometer integer overflow with Arduino Due

Question

I am working on a self-balancing robot project and am using the ADXL345 accelerometer alongside the ITG-3200 gyro to sense the tilt of the robot.

I was having problems earlier with signed integers not displaying correctly, as it turns out, the Arduino Due stores standard integers as 32 bit numbers which resulted in the 16 bit signed data coming out all positive and over 65000 for negatives. I did some research and have implemented int16_t throughout to maintain a signed 16 bit system however I have come across a new issue.

It seems that after the robot is tilted any more than 35 degrees or so in either direction the integers begin to overflow and any numbers over 32,767 begin to decrease again. This obviously would not be suitable when determining the angle of the robot. Is there any way to scale back these values or otherwise receive a constant stream of valid data for the complete range?

See code below:

#include <stdint.h>

int16_t accel_x;
int16_t accel_y;
int16_t accel_z;

int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;


void setup()
{
  // Init serial output
  Serial.begin(57600);

    // Init sensors
  delay(50);  // Give sensors enough time to start
  I2C_Init();
  Accel_Init();
  Gyro_Init();
}

void loop()
{
  Read_Accel();
  Serial.print("#A:");
  Serial.print(accel_x); Serial.print(",");
  Serial.print(accel_y); Serial.print(",");
  Serial.print(accel_z); Serial.println();

  Read_Gyro();
  Serial.print("#G:");
  Serial.print(gyro_x); Serial.print(",");
  Serial.print(gyro_y); Serial.print(",");
  Serial.print(gyro_z); Serial.println();
}

// *******************I2C code to read the sensors************************
#include <Wire.h>

// Sensor I2C addresses
#define ACCEL_ADDRESS ((int) 0x53) // 0x53 = 0xA6 / 2
#define GYRO_ADDRESS  ((int) 0x68) // 0x68 = 0xD0 / 2


void I2C_Init()
{
  Wire.begin();
}

void Accel_Init()
{
  Wire.beginTransmission(ACCEL_ADDRESS);
  Wire.write(0x2D);  // Power register
  Wire.write(0x08);  // Measurement mode
  Wire.endTransmission();
  delay(5);
  Wire.beginTransmission(ACCEL_ADDRESS);
  Wire.write(0x31);  // Data format register
  Wire.write(0x08);  // Set to full resolution
  Wire.endTransmission();
  delay(5);

  // Adjust the output data rate to 100Hz (50Hz bandwidth)
  Wire.beginTransmission(ACCEL_ADDRESS);
  Wire.write(0x2C);  // Rate
  Wire.write(0x0A);  // Set to 100Hz, normal operation
  Wire.endTransmission();
  delay(5);
}

// Reads x, y and z accelerometer registers
void Read_Accel()
{
  int i = 0;
  byte buff[6];

  Wire.beginTransmission(ACCEL_ADDRESS); 
  Wire.write(0x32);  // Send address to read from
  Wire.endTransmission();

  Wire.beginTransmission(ACCEL_ADDRESS);
  Wire.requestFrom(ACCEL_ADDRESS, 6);  // Request 6 bytes
  while(Wire.available())  // ((Wire.available())&&(i<6))
  { 
    buff[i] = Wire.read();  // Read one byte
    i++;
  }
  Wire.endTransmission();
  {
    accel_x = (( buff[2]) << 8) | buff[3];  // X axis (internal sensor y axis)
    accel_y = (( buff[0]) << 8) | buff[1];  // Y axis (internal sensor x axis)
    accel_z = (( buff[4]) << 8) | buff[5];  // Z axis (internal sensor z axis)
  }
}

void Gyro_Init()
{
  // Power up reset defaults
  Wire.beginTransmission(GYRO_ADDRESS);
  Wire.write(0x3E);
  Wire.write(0x80);
  Wire.endTransmission();
  delay(5);

  // Select full-scale range of the gyro sensors
  // Set LP filter bandwidth to 42Hz
  Wire.beginTransmission(GYRO_ADDRESS);
  Wire.write(0x16);
  Wire.write(0x1B);  // DLPF_CFG = 3, FS_SEL = 3
  Wire.endTransmission();
  delay(5);

  // Set sample rato to 100Hz
  Wire.beginTransmission(GYRO_ADDRESS);
  Wire.write(0x15);
  Wire.write(0x09);  //  SMPLRT_DIV = 9 (100Hz)
  Wire.endTransmission();
  delay(5);

  // Set clock to PLL with z gyro reference
  Wire.beginTransmission(GYRO_ADDRESS);
  Wire.write(0x3E);
  Wire.write(0x00);
  Wire.endTransmission();
  delay(5);
}

// Reads x, y and z gyroscope registers
void Read_Gyro()
{
  int i = 0;
  byte buff[6];

  Wire.beginTransmission(GYRO_ADDRESS); 
  Wire.write(0x1D);  // Sends address to read from
  Wire.endTransmission();

  Wire.beginTransmission(GYRO_ADDRESS);
  Wire.requestFrom(GYRO_ADDRESS, 6);  // Request 6 bytes
  while(Wire.available())  // ((Wire.available())&&(i<6))
  { 
    buff[i] = Wire.read();  // Read one byte
    i++;
  }
  Wire.endTransmission();
  {
    gyro_x = (((buff[2]) << 8) | buff[3]);    // X axis (internal sensor -y axis)
    gyro_y = (((buff[0]) << 8) | buff[1]);    // Y axis (internal sensor -x axis)
    gyro_z = (((buff[4]) << 8) | buff[5]);    // Z axis (internal sensor -z axis)
  }
}

Answer 1

I think it can be a problem of the byte format. Than the compiler realy use a the byte from buff[2] the byte (8bit) will be shifted by 8 to nirwana. The result will be 0 and this will be or-combined with buff[3]. The compiler works one step to next.

original:

byte buff[6];

accel_x = (( buff[2]) << 8) | buff[3];  // X axis (internal sensor y axis)

solution:

accel_x = (( (int16_t)buff[2]) << 8) | buff[3];  // X axis (internal sensor y axis)

or in steps

accel_x= buff[2];//accel_x has 16-bit-format
accel_x<<= 8;
accel_x|= buff[3];