I am writing a project that involves using complementary filters to combine gyroscope and accelerator readings in 3 directions. The relevant code is below.

class CompFilter {
 public:
   long prevTime;
   float alpha;
   int angle;

   CompFilter(float Alph);
   int combine (float gyroRate, float accelAngle);
};

CompFilter::CompFilter(float Alph) {
 alpha = Alph;
 prevTime = 0;
 angle = 0;
}

int CompFilter::combine(float gyroRate, float accelAngle){
  Serial.println("Here THREE");

 long timeStep = millis() - prevTime;
 if (timeStep == 0) { timeStep = 1;}

 angle = alpha * (angle + gyroRate * timeStep);

 angle += (1.0 - alpha) * accelAngle;

 prevTime = millis();

 return angle;
}

//Global Objects
CompFilter *senPitch; CompFilter *senRoll;  //Declare Complementary Filters


//Adjustable Parameters
float alpha = .5;   //Constant affecting Gyro's vs. Accel's influence on sensor reading

void setup(void)
{
  //Open Data Channel
  Serial.begin(115200);
  Serial.println(F("IMU Complementary Filter Tester")); Serial.println("");

 //Initialise TMU sensors
 sensorInit(accel, mag, gyro);
 senPitch = new CompFilter(alpha); senPitch = new CompFilter(alpha);
  Serial.println("HERE ONE");
}

void loop(void)
{
 //Variables for reading sensors
 sensors_event_t accel_event, mag_event, gyro_event;
 sensors_vec_t   orientation;

 // Retrieve Sensor Readings
 accel.getEvent(&accel_event);
 gyro.getEvent(&gyro_event);
 mag.getEvent(&mag_event);

 Serial.println("HERE TWO");

 int anglePitch = senPitch->combine(gyro_event.gyro.y, orientation.pitch);
 int angleRoll = senRoll->combine(gyro_event.gyro.x, orientation.roll);

}

I had to keep track of a couple of pieces of information for each direction, so rather than trying to use parallel arrays, I created a CompFilter class. I think I am using the correct syntax in the class and constructing the object, but I am getting very unusual outputs from Serial.print().

Depending on how I fiddle with the code, I can get setup() repeating over and over or loop running once before silently failing.

I suspect that I have misdirected a pointer somewhere, but I am not sure. I would appreciate any help.

I am writing a project that involves using complementary filters to combine gyroscope and accelerator readings in 3 directions. The relevant code is below.

class CompFilter {
 public:
   long prevTime;
   float alpha;
   int angle;

   CompFilter(float Alph);
   int combine (float gyroRate, float accelAngle);
};

CompFilter::CompFilter(float Alph) {
 alpha = Alph;
 prevTime = 0;
 angle = 0;
}

int CompFilter::combine(float gyroRate, float accelAngle){
  Serial.println("Here THREE");

 long timeStep = millis() - prevTime;
 if (timeStep == 0) { timeStep = 1;}

 angle = alpha * (angle + gyroRate * timeStep);

 angle += (1.0 - alpha) * accelAngle;

 prevTime = millis();

 return angle;
}

//Global Objects
CompFilter *senPitch; CompFilter *senRoll;  //Declare Complementary Filters


//Adjustable Parameters
float alpha = .5;   //Constant affecting Gyro's vs. Accel's influence on sensor reading

void setup(void)
{
  //Open Data Channel
  Serial.begin(115200);
  Serial.println(F("IMU Complementary Filter Tester")); Serial.println("");

 //Initialise TMU sensors
 sensorInit(accel, mag, gyro);
 senPitch = new CompFilter(alpha); senPitch = new CompFilter(alpha);
  Serial.println("HERE ONE");
}

void loop(void)
{
 //Variables for reading sensors
 sensors_event_t accel_event, mag_event, gyro_event;
 sensors_vec_t   orientation;

 // Retrieve Sensor Readings
 accel.getEvent(&accel_event);
 gyro.getEvent(&gyro_event);
 mag.getEvent(&mag_event);

 Serial.println("HERE TWO");

 int anglePitch = senPitch->combine(gyro_event.gyro.y, orientation.pitch);
 int angleRoll = senRoll->combine(gyro_event.gyro.x, orientation.roll);

}

I had to keep track of a couple of pieces of information for each direction, so rather than trying to use parallel arrays, I created a CompFilter class. I think I am using the correct syntax in the class and constructing the object, but I am getting very unusual outputs from Serial.print().

RE ONE
RE TWO
Here THREE
Here THREE
entary Filter Tester

Depending on how I fiddle with the code, I can get setup() repeating over and over or loop() running once before silently failing.

I suspect that I have misdirected a pointer somewhere, but I am not sure. I would appreciate any help.