BROKKING lsm6

#include <Wire.h>

#define DEBUG


const float RefreshFrequency = 250.00, // CONFIG
            PololuRefreshFrequency = 208.00;

unsigned short RefreshRate = round((float)1 / RefreshFrequency * 1000),
               SensorRefreshRate = round((float)1 / PololuRefreshFrequency * 1000); // frequencies in miiliseconds

unsigned long update_timer = 0,
              sensor_update = 0,
              print_timer = 0;

const short CALIBRATION_PROBES = 100;

const float GYRO_SENSITIVITY = 0.0175, // mdps/LSB -> dps/LSB
            COMPLIMENTARY_FACTOR = 0.7; // < 1

long  C[6] = {0,0,0,0,0,0};

long gx, gy, gz,
     ax, ay, az;

const float RateDeltaTime = GYRO_SENSITIVITY / RefreshFrequency,
            RateDeltaTimeDegress = RateDeltaTime * PI / 180,
            GRAVITY = 256.0;

bool calibration_finished = false;

float g_pitch, g_roll, g_yaw,
      angle_pitch, angle_roll,
      acc_vector, a_pitch, a_roll;

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

#ifdef DEBUG
  Serial.begin(115200);
#endif

  PCICR  |= (1 << PCIE0);
  PCMSK0 |= (1 << PCINT4); // PIN 10
  PCMSK0 |= (1 << PCINT5); // PIN 11
  PCMSK0 |= (1 << PCINT6); // PIN 12
  PCMSK0 |= (1 << PCINT7); // PIN 13


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

  sensor_update = millis();
  for(short i = 0; i < CALIBRATION_PROBES; i++)
  {
    Read();

    #ifdef DEBUG
      if(i % 50 == 0) Serial.print(".");
     #endif

    C[0] += gx;
    C[1] += gy;
    C[2] += gz;

    C[3] += ax;
    C[4] += ay;
    C[5] += az;

    delay(5);
  }

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

  C[5] -= GRAVITY;

  calibration_finished = true;

  update_timer = print_timer = millis();

#ifdef DEBUG
  Serial.print("RefreshRate: "); Serial.println(RefreshRate);
#endif

}

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


bool Read()
{
  unsigned long current_timer = millis();

  if(current_timer - sensor_update < SensorRefreshRate)
    return false;

  sensor_update = current_timer;

  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) >> 4; // mg
  ay = (long)(ayh << 8 | ayl) >> 4; // mg
  az = (long)(azh << 8 | azl) >> 4; // mg

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

  acc_vector = sqrt(ax * ax + ay * ay + az * az);
  // acc_vector /= GRAVITY;

  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(calibration_finished)
  {
    gx -= C[0];
    gy -= C[1];
    gz -= C[2];
  }

#ifdef BROKKING_ALGO
  g_pitch = (g_pitch * COMPLIMENTARY_FACTOR) + ((float)gx * GYRO_SENSITIVITY) * (1 - COMPLIMENTARY_FACTOR);
  g_roll = (g_roll * COMPLIMENTARY_FACTOR) + ((float)gy * GYRO_SENSITIVITY) * (1 - COMPLIMENTARY_FACTOR);
  g_yaw = (g_yaw * COMPLIMENTARY_FACTOR) + ((float)gz * GYRO_SENSITIVITY) * (1 - COMPLIMENTARY_FACTOR);
#endif

  return true;
}

void loop()
{

  unsigned long current_clock = millis();
  Read();

  angle_pitch += (float)gx * RateDeltaTime;
  angle_roll += (float)gy * RateDeltaTime;

  angle_pitch -= angle_roll * sin(gz * RateDeltaTimeDegress);
  angle_roll += angle_pitch * sin(gz * RateDeltaTimeDegress);

  if(abs(ay) < acc_vector)
    a_pitch = asin((float)ay / acc_vector) * 57.296;

  if(abs(ax) < acc_vector)
    a_roll = asin((float)ax / acc_vector) * -57.296;

  angle_pitch = angle_pitch * 0.9996 + a_pitch * 0.0004;
  angle_roll = angle_roll * 0.9996 + a_roll * 0.0004;
  // current_clock = micros();

#ifdef DEBUG
  //if(current_clock - print_timer >= 500)
  {
    Serial.print("ACC X:" ); Serial.print(ax); Serial.print(" Y:" ); Serial.print(ay); Serial.print(" Z:" ); Serial.println(az);
    Serial.print("GYRO X:" ); Serial.print(gx); Serial.print(" Y:" ); Serial.print(gy); Serial.print(" Z:" ); Serial.println(gz);
   // Serial.print("PITCH: "); Serial.print(round(angle_pitch)); Serial.print(" ROLL: "); Serial.println(round(angle_roll));
    print_timer = current_clock;
  }
#endif

 while(millis() - update_timer < 4);
 update_timer = millis();
}