/**
 * @file mpu6050.h
 * @author zzy
 * @brief mpu6050驱动头文件
 * @version 0.1
 * @date 2023-05-11
 *
 * @copyright Copyright (c) 2023
 *
 */
#ifndef _MPU6050_H
#define _MPU6050_H

#include "main.h"
#include <stdlib.h>
#include <math.h>
#include "i2c.h"

#define RAD_TO_DEG 57.3

#define WHO_AM_I_REG 0x75
#define PWR_MGMT_1_REG 0x6B
#define SMPLRT_DIV_REG 0x19
#define ACCEL_CONFIG_REG 0x1C
#define ACCEL_XOUT_H_REG 0x3B
#define TEMP_OUT_H_REG 0x41
#define GYRO_CONFIG_REG 0x1B
#define GYRO_XOUT_H_REG 0x43

IIC iic(GPIOB, GPIO_PIN_6, GPIOB, GPIO_PIN_7);

/**
 * @brief MPU6050数据类型结构体
 *
 */
typedef struct
{

    int16_t Accel_X_RAW;
    int16_t Accel_Y_RAW;
    int16_t Accel_Z_RAW;
    double Ax;
    double Ay;
    double Az;

    int16_t Gyro_X_RAW;
    int16_t Gyro_Y_RAW;
    int16_t Gyro_Z_RAW;
    double Gx;
    double Gy;
    double Gz;

    float Temperature;

    double KalmanAngleX;
    double KalmanAngleY;
} MPU6050_t;

/**
 * @brief Kalman滤波所用数据结构体
 *
 */
typedef struct
{
    double Q_angle;
    double Q_bias;
    double R_measure;
    double angle;
    double bias;
    double P[2][2];
} Kalman_t;

/**
 * @brief 驱动MPU6050的类
 *
 */
class MPU6050
{
private:
    uint16_t __addr__;
    uint8_t WriteByte(uint8_t reg, uint8_t data);
    uint8_t ReadByte(uint8_t reg);
public:
    MPU6050(uint16_t addr);
    uint8_t init();
    void Read_Accel(MPU6050_t *DataStruct);
    void Read_Gyro(MPU6050_t *DataStruct);
    void Read_Temp(MPU6050_t *DataStruct);
    void Read_All(MPU6050_t *DataStruct);
    double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt);
};
#endif