/*
 * MPU6050.c
 *
 *  Created on: Oct 26, 2021
 *      Author: Administrator
 */

#include "MPU6050.h"
#include "main.h"
#include <stdio.h>

extern int t;
extern int a;
extern int b;
extern int c;

//void addr_check_point(u8 addr){
//    if (addr != 0x69)
//    {
//        printf("checkpoint error addr");
//    }
//}

//void common_check_point(u8 value){
//printf("<checkpoint> value => %x\n", value);
//}


u8 IIC_Write_Byte(u8 reg, u8 value)
{

	

    return IIC_Write_Len(a, reg, 1, &value);
}


u8 IIC_Read_Byte(u8 reg)
{
    u8 value = 0;


  	IIC_Read_Len(a, reg, 1, &value);

    return value;
}




u8 IIC_Write_Len1(u8 addr, u8 reg, u8 len, u8 *buf)
{
	
	addr=a;
	addr = (addr << 1) | 0;

        u8 data[10] = {0};
        data[0] = reg;
        for (int i = 0; i < len; i++)
        {
            data[1 + i] = buf[i];
        }

        if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)addr, (uint8_t *)data, len + 1, 10000) != HAL_OK
        ||HAL_I2C_Master_Transmit(&hi2c2, (uint16_t)addr, (uint8_t *)data, len + 1, 10000) != HAL_OK
        ||HAL_I2C_Master_Transmit(&hi2c3, (uint16_t)addr, (uint8_t *)data, len + 1, 10000) != HAL_OK)
        {
            printf("++write len byte fail\r\n");
            return 1;
					
        }
	return 0;
}
// IIC连续写
u8 IIC_Write_Len(u8 addr, u8 reg, u8 len, u8 *buf)
{
	
	addr=a;
  		

    if (t == 1)
    {
        addr = (addr << 1) | 0;

        u8 data[10] = {0};
        data[0] = reg;
        for (int i = 0; i < len; i++)
        {
            data[1 + i] = buf[i];
        }

        if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)addr, (uint8_t *)data, len + 1, 10000) != HAL_OK)
        {
            printf("++write len byte fail\r\n");
            return 1;
        }
    }
    else if (t == 2)
    {
        addr = (addr << 1) | 0;

        u8 data[10] = {0};
        data[0] = reg;
        for (int i = 0; i < len; i++)
        {
            data[1 + i] = buf[i];
        }

        if (HAL_I2C_Master_Transmit(&hi2c2, (uint16_t)addr, (uint8_t *)data, len + 1, 10000) != HAL_OK)
        {
            printf("++write len byte fail\r\n");
            return 1;
        }
    }
    else if (t == 3)
    {
        addr = (addr << 1) | 0;

        u8 data[10] = {0};
        data[0] = reg;
        for (int i = 0; i < len; i++)
        {
            data[1 + i] = buf[i];
        }

        if (HAL_I2C_Master_Transmit(&hi2c3, (uint16_t)addr, (uint8_t *)data, len + 1, 10000) != HAL_OK)
        {
            printf("++write len byte fail\r\n");
            return 1;
        }
    }
    return 0;
}

u8 IIC_Read_Len1(u8 addr, u8 reg, u8 len, u8 *buf)
{addr=a;
	u8 tmp = (addr << 1) | 0;
        if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)tmp, (uint8_t *)&reg, 1, 10000) != HAL_OK
        ||HAL_I2C_Master_Transmit(&hi2c2, (uint16_t)tmp, (uint8_t *)&reg, 1, 10000) != HAL_OK
        ||HAL_I2C_Master_Transmit(&hi2c3, (uint16_t)tmp, (uint8_t *)&reg, 1, 10000) != HAL_OK)
        {
            printf("++write byte fail\r\n");
            return 1;
				
			  
				//	return addr_test;
        }

        tmp = (addr << 1) | 1;
        if (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)tmp, (uint8_t *)buf, len, 10000) != HAL_OK
        ||HAL_I2C_Master_Receive(&hi2c2, (uint16_t)tmp, (uint8_t *)buf, len, 10000) != HAL_OK
        ||HAL_I2C_Master_Receive(&hi2c3, (uint16_t)tmp, (uint8_t *)buf, len, 10000) != HAL_OK)
        {
					
            printf("--read byte fail\r\n");
				
			  
        }
	
}

// IIC连续读
u8 IIC_Read_Len(u8 addr, u8 reg, u8 len, u8 *buf)
{
	
	addr=a;


    if (t == 1)
    {
        u8 tmp = (addr << 1) | 0;
        if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)tmp, (uint8_t *)&reg, 1, 10000) != HAL_OK)
        {
            printf("++write byte fail\r\n");
            return 1;
				//	return addr_test;
        }

        tmp = (addr << 1) | 1;
        if (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)tmp, (uint8_t *)buf, len, 10000) != HAL_OK)
        {
            printf("--read byte fail\r\n");
        }
    }
    else if (t == 2)
    {
        u8 tmp = (addr << 1) | 0;
        if (HAL_I2C_Master_Transmit(&hi2c2, (uint16_t)tmp, (uint8_t *)&reg, 1, 10000) != HAL_OK)
        {
            printf("++write byte fail\r\n");
            return 1;
				//	return addr_test;
        }

        tmp = (addr << 1) | 1;
        if (HAL_I2C_Master_Receive(&hi2c2, (uint16_t)tmp, (uint8_t *)buf, len, 10000) != HAL_OK)
        {
            printf("--read byte fail\r\n");
        }
    }
    else if (t == 3)
    {
        u8 tmp = (addr << 1) | 0;
        if (HAL_I2C_Master_Transmit(&hi2c3, (uint16_t)tmp, (uint8_t *)&reg, 1, 10000) != HAL_OK)
        {
            printf("++write byte fail\r\n");
             return 1;
				//	return addr_test;
        }

        tmp = (addr << 1) | 1;
        if (HAL_I2C_Master_Receive(&hi2c3, (uint16_t)tmp, (uint8_t *)buf, len, 10000) != HAL_OK)
        {
            printf("--read byte fail\r\n");
        }
    }
		
	//	return -1;
}

// 初始化MPU6050
// 返回值: 0,成功
//         其他,错误代码
u8 MPU_Init(void)
{
    u8 res = 0;

    IIC_Write_Byte(MPU_PWR_MGMT1_REG, 0X80); // 复位MPU6050

    HAL_Delay(100);

    IIC_Write_Byte(MPU_PWR_MGMT1_REG, 0X00); // 唤醒MPU6050

    MPU_Set_Gyro_Fsr(3);                     // 陀螺仪传感器,±2000dps
    MPU_Set_Accel_Fsr(0);                    // 加速度传感器 ±2g
    MPU_Set_Rate(50);                        // 设置采样率50HZ
    IIC_Write_Byte(MPU_INT_EN_REG, 0X00);    // 关闭所有中断
    IIC_Write_Byte(MPU_USER_CTRL_REG, 0X00); // I2C主模式关闭
    IIC_Write_Byte(MPU_FIFO_EN_REG, 0X00);   // 关闭FIFO
    IIC_Write_Byte(MPU_INTBP_CFG_REG, 0X80); // INT引脚低电平有效

    res = IIC_Read_Byte(MPU_DEVICE_ID_REG);

    printf("the addr is:%d\r\n", res);

    //    if(res==a)//器件ID正确
    //    {
    IIC_Write_Byte(MPU_PWR_MGMT1_REG, 0X01); // 设置CLKSEL,PLL X 轴为参考
    IIC_Write_Byte(MPU_PWR_MGMT2_REG, 0X00); // 加速度陀螺仪都工作
    MPU_Set_Rate(50);                        // 设置采样率为50HZ
    //    }
    //    else
    //    {
    //    	return 1;
    //    }

    return 0;
}

// 设置MPU6050陀螺仪传感器满量程范围
// fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
// 返回值:0,设置成功
//     其他,设置失败
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
    return IIC_Write_Byte(MPU_GYRO_CFG_REG, fsr << 3); // 设置陀螺仪满量程范围
}

// 设置MPU6050加速度传感器满量程范围
// fsr:0,±2g;1,±4g;2,±8g;3,±16g
// 返回值:0,设置成功
//     其他,设置失败
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
    return IIC_Write_Byte(MPU_ACCEL_CFG_REG, fsr << 3); // 设置加速度传感器满量程范围
}

// 设置MPU6050的数字低通滤波器
// lpf:数字低通滤波频率(Hz)
// 返回值:0,设置成功
//     其他,设置失败
u8 MPU_Set_LPF(u16 lpf)
{
    u8 data = 0;
    if (lpf >= 188)
        data = 1;
    else if (lpf >= 98)
        data = 2;
    else if (lpf >= 42)
        data = 2;
    else if (lpf >= 42)
        data = 3;
    else if (lpf >= 20)
        data = 4;
    else if (lpf >= 10)
        data = 5;
    else
        data = 6;
    return IIC_Write_Byte(MPU_CFG_REG, data); // 设置数字低通滤波器
}

// 设置MPU6050的采样率(假定Fs=1KHz)
// rate:4~1000(Hz)
// 返回值:0,设置成功
//     其他,设置失败
u8 MPU_Set_Rate(u16 rate)
{
    u8 data;
    if (rate > 1000)
        rate = 1000;
    if (rate < 4)
        rate = 4;
    data = 1000 / rate - 1;
    data = IIC_Write_Byte(MPU_SAMPLE_RATE_REG, data); // 设置数字低通滤波器
    return MPU_Set_LPF(rate / 2);                     // 自动设置LPF为采样率的一半
}

// 得到温度值
// 返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
    u8 buf[2];
    short raw;
    float temp;
    IIC_Read_Len(a, MPU_TEMP_OUTH_REG, 2, buf);
    raw = ((u16)buf[0] << 8) | buf[1];
    temp = 36.53 + ((double)raw) / 340;

    return temp * 100;
    ;
}

// 得到陀螺仪值(原始值)
// gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
// 返回值:0,成功
//  其他,错误代码
u8 MPU_Get_Gyroscope(short *gx, short *gy, short *gz)
{
    u8 buf[6], res;
    res = IIC_Read_Len(a, MPU_GYRO_XOUTH_REG, 6, buf);
    if (res == 0)
    {
        *gx = ((u16)buf[0] << 8) | buf[1];
        *gy = ((u16)buf[2] << 8) | buf[3];
        *gz = ((u16)buf[4] << 8) | buf[5];
    }
    return res;
}

// 得到加速度值(原始值)
// ax,ay,az:陀螺仪x,y,z轴的原始读数(带符号)
// 返回值:0,成功
//     其他,错误代码
u8 MPU_Get_Accelerometer(short *ax, short *ay, short *az)
{
    u8 buf[6], res;
    res = IIC_Read_Len(a, MPU_ACCEL_XOUTH_REG, 6, buf);
    if (res == 0)
    {
        *ax = ((u16)buf[0] << 8) | buf[1];
        *ay = ((u16)buf[2] << 8) | buf[3];
        *az = ((u16)buf[4] << 8) | buf[5];
    }
    return res;
    ;
}

/*--------------------以下为使用板载固件DMP的相关代码，这个融合得到的姿态角效果很好*/

/**
 * @brief  获取当前毫秒值
 * @param  存储最新毫秒值的变量
 * @retval 无
 */
int get_tick_count(unsigned long *count)
{
    count[0] = HAL_GetTick();
    return 0;
}

#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

/* Data requested by client. */
#define PRINT_ACCEL (0x01)
#define PRINT_GYRO (0x02)
#define PRINT_QUAT (0x04)

#define ACCEL_ON (0x01)
#define GYRO_ON (0x02)

#define MOTION (0)
#define NO_MOTION (1)

/* Starting sampling rate. */ // 100
#define DEFAULT_MPU_HZ (50)

struct rx_s
{
    unsigned char header[3];
    unsigned char cmd;
};
struct hal_s
{
    unsigned char sensors;
    unsigned char dmp_on;
    unsigned char wait_for_tap;
    volatile unsigned char new_gyro;
    unsigned short report;
    unsigned short dmp_features;
    unsigned char motion_int_mode;
    struct rx_s rx;
};
static struct hal_s hal = {0};

/* USB RX binary semaphore. Actually, it's just a flag. Not included in struct
 * because it's declared extern elsewhere.
 */
volatile unsigned char rx_new;

/* The sensors can be mounted onto the board in any orientation. The mounting
 * matrix seen below tells the MPL how to rotate the raw data from thei
 * driver(s).
 * TODO: The following matrices refer to the configuration on an internal test
 * board at Invensense. If needed, please modify the matrices to match the
 * chip-to-body matrix for your particular set up.
 */
static signed char gyro_orientation[9] = {-1, 0, 0,
                                          0, -1, 0,
                                          0, 0, 1};

/* Every time new gyro data is available, this function is called in an
 * ISR context. In this example, it sets a flag protecting the FIFO read
 * function.
 */
static void gyro_data_ready_cb(void)
{
    hal.new_gyro = 1;
}

enum packet_type_e
{
    PACKET_TYPE_ACCEL,
    PACKET_TYPE_GYRO,
    PACKET_TYPE_QUAT,
    PACKET_TYPE_TAP,
    PACKET_TYPE_ANDROID_ORIENT,
    PACKET_TYPE_PEDO,
    PACKET_TYPE_MISC
};

#define BYTE u_char
/* Send data to the Python client application.
 * Data is formatted as follows:
 * packet[0]    = $
 * packet[1]    = packet type (see packet_type_e)
 * packet[2+]   = data
 */
void send_packet(char packet_type, void *data)
{

#define MAX_BUF_LENGTH (18)
    char buf[MAX_BUF_LENGTH], length;

    memset(buf, 0, MAX_BUF_LENGTH);
    buf[0] = '$';
    buf[1] = packet_type;

    if (packet_type == PACKET_TYPE_ACCEL || packet_type == PACKET_TYPE_GYRO)
    {
        short *sdata = (short *)data;
        buf[2] = (char)(sdata[0] >> 8);
        buf[3] = (char)sdata[0];
        buf[4] = (char)(sdata[1] >> 8);
        buf[5] = (char)sdata[1];
        buf[6] = (char)(sdata[2] >> 8);
        buf[7] = (char)sdata[2];
        length = 8;
    }
    else if (packet_type == PACKET_TYPE_QUAT)
    {
        long *ldata = (long *)data;
        buf[2] = (char)(ldata[0] >> 24);
        buf[3] = (char)(ldata[0] >> 16);
        buf[4] = (char)(ldata[0] >> 8);
        buf[5] = (char)ldata[0];
        buf[6] = (char)(ldata[1] >> 24);
        buf[7] = (char)(ldata[1] >> 16);
        buf[8] = (char)(ldata[1] >> 8);
        buf[9] = (char)ldata[1];
        buf[10] = (char)(ldata[2] >> 24);
        buf[11] = (char)(ldata[2] >> 16);
        buf[12] = (char)(ldata[2] >> 8);
        buf[13] = (char)ldata[2];
        buf[14] = (char)(ldata[3] >> 24);
        buf[15] = (char)(ldata[3] >> 16);
        buf[16] = (char)(ldata[3] >> 8);
        buf[17] = (char)ldata[3];
        length = 18;
    }
    else if (packet_type == PACKET_TYPE_TAP)
    {
        buf[2] = ((char *)data)[0];
        buf[3] = ((char *)data)[1];
        length = 4;
    }
    else if (packet_type == PACKET_TYPE_ANDROID_ORIENT)
    {
        buf[2] = ((char *)data)[0];
        length = 3;
    }
    else if (packet_type == PACKET_TYPE_PEDO)
    {
        long *ldata = (long *)data;
        buf[2] = (char)(ldata[0] >> 24);
        buf[3] = (char)(ldata[0] >> 16);
        buf[4] = (char)(ldata[0] >> 8);
        buf[5] = (char)ldata[0];
        buf[6] = (char)(ldata[1] >> 24);
        buf[7] = (char)(ldata[1] >> 16);
        buf[8] = (char)(ldata[1] >> 8);
        buf[9] = (char)ldata[1];
        length = 10;
    }
    else if (packet_type == PACKET_TYPE_MISC)
    {
        buf[2] = ((char *)data)[0];
        buf[3] = ((char *)data)[1];
        buf[4] = ((char *)data)[2];
        buf[5] = ((char *)data)[3];
        length = 6;
    }

    //    cdcSendDataWaitTilDone((BYTE*)buf, length, CDC0_INTFNUM, 100);
}

/* These next two functions converts the orientation matrix (see
 * gyro_orientation) to a scalar representation for use by the DMP.
 * NOTE: These functions are borrowed from Invensense's MPL.
 */
static inline unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7; // error
    return b;
}
static inline unsigned short inv_orientation_matrix_to_scalar(
    const signed char *mtx)
{
    unsigned short scalar;

    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;

    return scalar;
}

static inline void run_self_test(void)
{
    int result;
    char test_packet[4] = {0};
    long gyro[3], accel[3];

    result = mpu_run_self_test(gyro, accel);
    if (result == 0x7)
    {
        /* Test passed. We can trust the gyro data here, so let's push it down
         * to the DMP.
         */
        float sens;
        unsigned short accel_sens;
        mpu_get_gyro_sens(&sens);
        gyro[0] = (long)(gyro[0] * sens);
        gyro[1] = (long)(gyro[1] * sens);
        gyro[2] = (long)(gyro[2] * sens);
        dmp_set_gyro_bias(gyro);
        mpu_get_accel_sens(&accel_sens);
        accel[0] *= accel_sens;
        accel[1] *= accel_sens;
        accel[2] *= accel_sens;
        dmp_set_accel_bias(accel);
    }

    /* Report results. */
    test_packet[0] = 't';
    test_packet[1] = result;
    send_packet(PACKET_TYPE_MISC, test_packet);
}

#define INT_EXIT_LPM0 12

void DMP_Init(void)
{
      	        printf("checkpoint Func DMP_Init\n");
struct int_param_s int_param;

    /* Set up gyro.
     * Every function preceded by mpu_ is a driver function and can be found
     * in inv_mpu.h.
     */
    int_param.cb = gyro_data_ready_cb;
    int_param.pin = 16;
    int_param.lp_exit = INT_EXIT_LPM0;
    int_param.active_low = 1;

    int result = mpu_init(&int_param);

    printf("mpu_init, %d\r\n", result);

    if (result == 0) // mpu初始化
    {
  if (!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) // 设置需要的传感器
            printf("mpu_set_sensor complete ......\r\n");

        if (!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) // 设置fifo
            printf("mpu_configure_fifo complete ......\r\n");

        if (!mpu_set_sample_rate(DEFAULT_MPU_HZ)) // 设置采集样率
            printf("mpu_set_sample_rate complete ......\r\n");

        unsigned short gyro_rate = 0, gyro_fsr = 0;
        unsigned char accel_fsr = 0;

        mpu_get_sample_rate(&gyro_rate);
        mpu_get_gyro_fsr(&gyro_fsr);
        mpu_get_accel_fsr(&accel_fsr);

        printf("gyro_rate %d, gyro_fsr %d, accel_fsr %d\r\n", gyro_rate, gyro_fsr, accel_fsr);

        if (!dmp_load_motion_driver_firmware()) // 加载dmp固件
            printf("dmp_load_motion_driver_firmware complete ......\r\n");

        if (!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
            printf("dmp_set_orientation complete ......\r\n"); // 设置陀螺仪方向

        if (!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
                                DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
                                DMP_FEATURE_GYRO_CAL))
            printf("dmp_enable_feature complete ......\r\n");

        if (!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) // 设置速率
            printf("dmp_set_fifo_rate complete ......\r\n");

        run_self_test(); // 自检(非必要)

        if (!mpu_set_dmp_state(1)) // 使能
            printf("mpu_set_dmp_state complete ......\r\n");
    }
}

#define q30 1073741824.0

uint8_t Read_DMP(float *Pitch, float *Roll, float *Yaw)
{

    short gyro[3], accel[3], sensors;
    float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
    unsigned long sensor_timestamp;
    unsigned char more;
    long quat[4];

    int ret = dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);

    //	printf("dmp_read_fifo, %d\r\n", ret);

    if (ret)
    {
        return 1;
    }

    if (sensors & INV_WXYZ_QUAT)
    {
        q0 = quat[0] / q30;
        q1 = quat[1] / q30;
        q2 = quat[2] / q30;
        q3 = quat[3] / q30;
        *Pitch = (float)asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;
        *Roll = (float)atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // roll
        *Yaw = (float)atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3;
        return 0;
    }
    else
    {
        return 2;
    }
}