KALMAN LSM6

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

const float GYRO_SENS = 70.f / 1000.f;
double roll, pitch, angle_roll, angle_pitch;

long gx, gy, gz, ax, ay, az, C[6] = {0,0,0,0,0,0};

unsigned long loop_timer;
const byte PROB = 100;

Kalman kPitch, kRoll;

bool calib = false;

void setup()
{
  Wire.begin();
  Wire.setClock(400000L);

  Serial.begin(115200);

  WriteReg(0x10, 0b01011100); // 208Hz refresh rate, +-8g anti-aliasing 200Hz
  WriteReg(0x11, 0b01011100); // 208Hz refresh rate, +- 2k dps, full-scale at 125dps off
  WriteReg(0x12, 0b00000100);

  for(byte i = 0; i < PROB; i++)
  {
    Read();
    C[0] += gx;
    C[1] += gy;
    C[2] += gz;
    C[3] += ax;
    C[4] += ay;
    C[5] += az;
    delay(4);
  }

  for(byte i = 0; i < 6; i++)
    C[i] /= PROB;

  C[5] -= 256; // GRAVITY 1g

  calib = true;

  delay(1000);

  loop_timer = micros();
}

void WriteReg(uint8_t reg, uint8_t val)
{
  Wire.beginTransmission(0b1101011);
  Wire.write(reg);
  Wire.write(val);
  Wire.endTransmission();
}


bool Read()
{
  Wire.beginTransmission(0b1101011);
  Wire.write(0x28); // OUTX_L_XL
  Wire.endTransmission();
  Wire.requestFrom(0b1101011, (uint8_t) 6);

  uint8_t axl = Wire.read(), axh = Wire.read(),
          ayl = Wire.read(), ayh = Wire.read(),
          azl = Wire.read(), azh = Wire.read();

  ax = (long)(axh << 8 | axl); // mg
  ay = (long)(ayh << 8 | ayl); // mg
  az = (long)(azh << 8 | azl); // mg

  if(calib)
  {
    ax -= C[3];
    ay -= C[4];
    az -= C[5];
  }
  // az -= 256;

  Wire.beginTransmission(0b1101011);
  Wire.write(0x22); // OUTX_L_G
  Wire.endTransmission();
  Wire.requestFrom(0b1101011, (uint8_t) 6);

  uint8_t gxl = Wire.read(), gxh = Wire.read(),
          gyl = Wire.read(), gyh = Wire.read(),
          gzl = Wire.read(), gzh = Wire.read();

  gx = (long)(gxh << 8 | gxl);
  gy = (long)(gyh << 8 | gyl);
  gz = (long)(gzh << 8 | gzl);

  if(calib)
  {
    gx -= C[0];
    gy -= C[1];
    gz -= C[2];

    gx *= GYRO_SENS;
    gy *= GYRO_SENS;
    gz *= GYRO_SENS;
  }
}

unsigned long print_loop = 0;
void loop()
{
  Read();

  float dt = (float)(micros() - loop_timer) / 1000000.f;
  pitch = (atan2(ay, az) + PI) * RAD_TO_DEG;
  roll = (atan2(az, az) + PI) * RAD_TO_DEG;

  angle_pitch = kPitch.getAngle(pitch, gy, dt);
  angle_roll = kRoll.getAngle(roll, gz, dt);
  loop_timer = micros();

  if(millis() - print_loop >= 1000)
  {
    Serial.print("Pitch: "); Serial.print(angle_pitch); Serial.print(" Roll: "); Serial.println(angle_roll);
    print_loop = millis();
  }

}