Untitled

#include "Wire.h"

const int digitalPin = 3; // Hall sensor digital interface
const int MPU_addr = 0x68; // MPU6050 I2C address
unsigned long previousMillis = 0;
const long interval = 10; // Interval for data read and output in milliseconds

/*
//COM3
float A_cal[6] = { 0, 0, 0, 1, 1, 1}; // naklon horedole,dopravadolava vert, dopravadolava horiz, vracia pohyby alebo ostava v pohybe v danych osiach
float G_off[3] = { -44, 13, 24.2};       // +rot hore a dole          asi vertikalne doprava dolava          +rot dolava,-rot doprava

//COM4
float A_cal[6] = { 1500, 500, 0, 1, 1, 1}; // naklon horedole,dopravadolava vert, dopravadolava horiz, vracia pohyby alebo ostava v pohybe v danych osiach
float G_off[3] = { 0, 0, 0};       // -0,2 ide do miinusu a -1000 ide este viac do minusu

//COM5
float A_cal[6] = { 0, 0, 0, 1, 1, 1}; // naklon horedole,dopravadolava vert, dopravadolava horiz, vracia pohyby alebo ostava v pohybe v danych osiach
float G_off[3] = { 0, 0, -100};       // -0,2 ide do miinusu a -1000 ide este viac do minusu
*/
//COM6
float A_cal[6] = { 0, 0, 0, 1, 1, 1}; // naklon horedole,dopravadolava vert, dopravadolava horiz, vracia pohyby alebo ostava v pohybe v danych osiach
float G_off[3] = { 0, 0, -201};       // -0,2 ide do miinusu a -1000 ide este viac do minusu
/*
//COM7
float A_cal[6] = { 0, 0, 0, 1, 1, 1}; // naklon horedole,dopravadolava vert, dopravadolava horiz, vracia pohyby alebo ostava v pohybe v danych osiach
float G_off[3] = { 0, 0, -47};       // -0,2 ide do miinusu a -1000 ide este viac do minusu
*/
const float gscale = (250.0 / 32768.0) * (PI / 180.0); // Gyro scale to radians/s

float q[4] = {1.0, 0.0, 0.0, 0.0}; // Quaternion for orientation
float Kp = 20, Ki = 0; // Mahony filter parameters

void setup() {
  Wire.begin();
  Serial.begin(115200);
  Serial.println("starting");
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  Wire.write(0); // Wake up MPU6050
  Wire.endTransmission(true);
}

void loop() {
  
  if (millis() - previousMillis >= interval) {
    readAndProcessSensorData();
    previousMillis = millis();
  }

  /*
  readAndProcessSensorData();
  delay(10);
  */
}

void readAndProcessSensorData() {
  int hallSensor = digitalRead(digitalPin);
  int16_t ax, ay, az, gx, gy, gz;
  readMPU6050(ax, ay, az, gx, gy, gz);
  float Axyz[3] = {(ax - A_cal[0]) * A_cal[3], (ay - A_cal[1]) * A_cal[4], (az - A_cal[2]) * A_cal[5]};
  float Gxyz[3] = {(gx - G_off[0]) * gscale, (gy - G_off[1]) * gscale, (gz - G_off[2]) * gscale};
  static float lastUpdate = micros() * 1.0e-6;
  float now = micros() * 1.0e-6;
  float deltat = now - lastUpdate;
  lastUpdate = now;
  Mahony_update(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], deltat);
  outputData(hallSensor);
}

void readMPU6050(int16_t &ax, int16_t &ay, int16_t &az, int16_t &gx, int16_t &gy, int16_t &gz) {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B); // Start with ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14); // Request 14 registers
  ax = (Wire.read() << 8) | Wire.read();
  ay = -(Wire.read() << 8) | Wire.read();
  az = (Wire.read() << 8) | Wire.read();
  Wire.read(); Wire.read(); // Skip temperature
  gx = (Wire.read() << 8) | Wire.read();
  gy = (Wire.read() << 8) | Wire.read();
  gz = (Wire.read() << 8) | Wire.read();
}

void outputData(int hallSensor) {
  if(hallSensor == HIGH){
    q[0] = 1;
    q[1] = 0;
    q[2] = 0;
    q[3] = 0;
    
    Serial.print(String(q[0]) + "/" + String(q[1]) + "/" + String(q[2]) + "/" + String(q[3]) + "/" + String(hallSensor) + "/" );

  }
  else{
    Serial.print(String(q[0]) + "/" + String(q[1]) + "/" + String(q[2]) + "/" + String(q[3]) + "/" + String(hallSensor) + "/" );
  }
  
}

// Mahony_update function remains unchanged
void Mahony_update(float ax, float ay, float az, float gx, float gy, float gz, float deltat) {
  float recipNorm;
  float vx, vy, vz;
  float ex, ey, ez;  //error terms
  float qa, qb, qc;
  static float ix = 0.0, iy = 0.0, iz = 0.0;  //integral feedback terms
  float tmp;
  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  tmp = ax * ax + ay * ay + az * az;
  if (tmp > 0.0)
  {
    // Normalise accelerometer (assumed to measure the direction of gravity in body frame)
    recipNorm = 1.0 / sqrt(tmp);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;
    
    // Estimated direction of gravity in the body frame (factor of two divided out)
    vx = q[1] * q[3] - q[0] * q[2];  //to normalize these terms, multiply each by 2.0
    vy = q[0] * q[1] + q[2] * q[3];
    vz = q[0] * q[0] - 0.5f + q[3] * q[3];

    // Error is cross product between estimated and measured direction of gravity in body frame
    // (half the actual magnitude)
    ex = (ay * vz - az * vy);
    ey = (az * vx - ax * vz);
    ez = (ax * vy - ay * vx);

    // Compute and apply to gyro term the integral feedback, if enabled
    if (Ki > 0.0f) {
      ix += Ki * ex * deltat;  // integral error scaled by Ki
      iy += Ki * ey * deltat;
      iz += Ki * ez * deltat;
      gx += ix;  // apply integral feedback
      gy += iy;
      gz += iz;
    }

    // Apply proportional feedback to gyro term
    gx += Kp * ex;
    gy += Kp * ey;
    gz += Kp * ez;
  }

  // Integrate rate of change of quaternion, q cross gyro term
  deltat = 0.5 * deltat;
  gx *= deltat;   // pre-multiply common factors
  gy *= deltat;
  gz *= deltat;
  qa = q[0];
  qb = q[1];
  qc = q[2];
  
  q[0] += (-qb * gx - qc * gy - q[3] * gz);
  q[1] += (qa * gx + qc * gz - q[3] * gy);
  q[2] += (qa * gy - qb * gz + q[3] * gx);
  q[3] += (qa * gz + qb * gy - qc * gx);

  // renormalise quaternion
  recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
  q[0] = q[0] * recipNorm;
  q[1] = q[1] * recipNorm;
  q[2] = q[2] * recipNorm;
  q[3] = q[3] * recipNorm;
}