#include "attitude.h"
#include "mpu6050.h"
#include "variables.h"
#include <math.h>

#define RAD2DEG  (57.2957795f)

#define ACC_LSB_PER_G       16384.0f
#define GYRO_LSB_PER_DPS    131.0f
#define M_PI 3.14159265358979323846f
#define ALPHA              0.98f

static float pitch = 0.0f;

void Attitude_SetZero(void)
{
    pitch_offset = pitch * RAD2DEG;
}

void Attitude_Init(void)
{
    if (MPU6050_Init() != 0)
    {
        pitch_deg = 0.0f;
        return;
    }

    pitch = 0.0f;
    pitch_deg = 0.0f;
}

void Attitude_Update(float dt)
{
    MPU6050_Raw_t r;
    if (MPU6050_ReadRaw(&r) != 0) return;

    float ax = (float)r.ax / ACC_LSB_PER_G;
    float ay = (float)r.ay / ACC_LSB_PER_G;
    float az = (float)r.az / ACC_LSB_PER_G;

    float gyro_y_dps = (float)r.gy / GYRO_LSB_PER_DPS;

    float pitch_acc = atan2f(-ax, sqrtf(ay*ay + az*az));

    float pitch_gyro = pitch + (gyro_y_dps * (float)M_PI / 180.0f) * dt;

    pitch = ALPHA * pitch_gyro + (1.0f - ALPHA) * pitch_acc;

    pitch_deg = pitch * RAD2DEG - pitch_offset;
}



