filterKalman.h.bkp

#include <Kalman.h>
#include "config.h"

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
timer = micros();

void init(struct IMU *accel) {
  kalmanX.setAngle(accel->roll); // Set starting angle
  kalmanY.setAngle(accel->pitch);
}

void setFilter(struct IMU *accel,struct IMU *gyro) {
  double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
  timer = micros();
  #ifdef RESTRICT_PITCH
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
    kalmanX.setAngle(roll);
    kalAngleX = roll;
  } else
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleX) > 90)
    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((accel->pitch < -90 && kalAngleY > 90) || (accel->pitch > 90 && kalAngleY < -90)) {
    kalmanY.setAngle(accel->pitch);
    kalAngleY = accel->pitch;
  } else
    kalAngleY = kalmanY.getAngle(accel->pitch, gyro->pitch, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleY) > 90)
     // Invert rate, so it fits the restriced accelerometer reading
  kalAngleX = kalmanX.getAngle(accel->roll, -gyro->pitch, dt); // Calculate the angle using a Kalman filter
#endif
}