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.