SOLVED: Complementary Filter Confusion

Hello All,

I have been working on my balancing robot project on and off since the new year and have made it as far as filtering the signals to provide a clean angle to work with when balancing the robot. A thorough explanation of this technique can be found here: http://web.mit.edu/scolton/www/filter.pdf. In the filter I have implemented (Based on the filter found here: pieter-jan.com - This website is for sale! - pieter jan Resources and Information.) I can read back an accurate accelerometer angle (pitchAcc) however when I go to read the combined angle (*pitch) I read only angles of 0.00 degrees. What is happening here?

#include <stdint.h>

#define GYROSCOPE_SENSITIVITY 14.375
#define M_PI 3.14159265359	    
#define dt 0.01	// 10 ms sample rate!  
float *pitch;
float pitchAcc;

int16_t accel_x;
int16_t accel_y;
int16_t accel_z;

int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;

int STD_LOOP_TIME = 9; //Length of main loop in milliseconds
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;

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();
  
  ComplementaryFilter();
  Serial.print("Angle: ");
  Serial.print(*pitch); Serial.println();
  
// *********************** loop timing control ***************************
  lastLoopUsefulTime = millis()-loopStartTime;
  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);
  lastLoopTime = millis() - loopStartTime;
  loopStartTime = millis();
}

// *******************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 = ((((int) buff[3]) << 8) | buff[2])-5;  // X axis (internal sensor y axis)
    accel_y = ((((int) buff[1]) << 8) | buff[0])-3;  // Y axis (internal sensor x axis)
    accel_z = ((((int) buff[5]) << 8) | buff[4])-1;  // 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 = ((((int) buff[2]) << 8) | buff[3])-11;    // X axis (internal sensor -y axis)
    gyro_y = ((((int) buff[0]) << 8) | buff[1])+34;    // Y axis (internal sensor -x axis)
    gyro_z = ((((int) buff[4]) << 8) | buff[5])+12;    // Z axis (internal sensor -z axis)
  }
}

// ***********************Complementary Filter****************************
void ComplementaryFilter()
{     
    // Integrate the gyroscope data -> int(angularSpeed) = angle
    *pitch = ((float)gyro_y / GYROSCOPE_SENSITIVITY) * dt; // Angle around the Y-axis
 
    // Compensate for drift with accelerometer data if wrong
    // Sensitivity = -2 to 2 G at 16Bit -> 2G = 513 && 0.5G = 128
    int16_t forceMagnitudeApprox = abs(accel_x) + abs(accel_y) + abs(accel_z);
    if (forceMagnitudeApprox > 128  && forceMagnitudeApprox < 513)
    {
	// Turning around the X axis results in a vector on the Y-axis
        pitchAcc = atan2f((float)accel_x, (float)accel_z) * 180 / M_PI;
        *pitch = *pitch * 0.98 + pitchAcc * 0.02;
    }
}

Any help would be appreciated

Andrew

Why is pitch a pointer to float? It should, it looks to me, be a float.

pitch should indeed not be a pointer - in fact you are dereferencing an uninitialized pointer which is a recipe for disaster.

Replace all the ' *pitch ' with just ' pitch '

That fixed part of the problem thanks and taught me yet more about the language, thank you! The only recurring problem now is that when the board is tilted by ±90 degrees, the angle I read back is only in about the ±2 degree range. Although I could potentially make this work, I wondered if I had simply mistaken a constant or conversion leading to the off angles?

Thanks,
Andrew

Never mind, I managed to solve that last one alone. I was missing a += and had written an = instead when initially defining the pitch with the gyro. Thank you all!