上半部分我们已经搞完传感器了，接下来我们就要开始编写读写传感器数据的程序了，根据课程学习，我们先定义一个结构体类型，用来存放加速度值、陀螺仪值以及姿态值。这个结构体，放到qmi8658c.h文件中

​
typedef struct{
    int16_t acc_y; //放xyz方向的加速度值
    int16_t acc_x;
    int16_t acc_z;
    int16_t gyr_y; //放xyz方向陀螺仪值
    int16_t gyr_x;
    int16_t gyr_z;
    float AngleX; //放XYZ的角度值
    float AngleY;
    float AngleZ;
}t_sQMI8658C;

​

这个结构体中用到了int16_t，需要包含stdint.h头文件

#include <stdint.h>

写读取加速度值和陀螺仪值的函数，放到qmi8658c.c文件中

void qmi8658c_Read_AccAndGry(t_sQMI8658C *p)
{
    uint8_t status, data_ready=0; //读取状态寄存器，看看加速度值和陀螺仪值是否可读
    int16_t buf[6]; //读到后的值，最终传入t_sQMI8658C定义的结构体

    qmi8658c_register_read(QMI8658C_STATUS0, &status, 1); // 读状态寄存器 
    if (status & 0x03) // 判断加速度和陀螺仪数据是否可读
    {
        data_ready = 1;
    }
    if (data_ready == 1)
    {
        data_ready = 0;
        qmi8658c_register_read(QMI8658C_AX_L, (uint8_t *)buf, 12); // 读加速度值
        p->acc_x = buf[0];
        p->acc_y = buf[1];
        p->acc_z = buf[2];
        p->gyr_x = buf[3];
        p->gyr_y = buf[4];
        p->gyr_z = buf[5];
    }
}

在这里，我们需要注意一下，这里面的buf数据变量，定义的时候是16位的6个元素，在读寄存器的时候，强制为8位指针变量，读12个字节。这里，大家可以看一下寄存器定义，加速度寄存器有6个，陀螺仪寄存器有6个，每个值都是由低字节寄存器和高字节寄存器组成，然后我们再写一个计算姿态的函数，计算姿态，可以单独使用加速度值，可以单独使用陀螺仪值，也可以融合使用，它们各自有优缺点，下面，我们写一个使用加速度值计算姿态的函数

void qmi8658c_fetch_angleFromAcc(t_sQMI8658C *p)
{
    float temp;

    qmi8658c_Read_AccAndGry(p);

    temp = (float)p->acc_x / sqrt( ((float)p->acc_y * (float)p->acc_y + (float)p->acc_z * (float)p->acc_z) );
    p->AngleX = atan(temp)*57.3f; // 180/3.14=57.3
    temp = (float)p->acc_y / sqrt( ((float)p->acc_x * (float)p->acc_x + (float)p->acc_z * (float)p->acc_z) );
    p->AngleY = atan(temp)*57.3f; // 180/3.14=57.3
    temp = (float)p->acc_z / sqrt( ((float)p->acc_x * (float)p->acc_x + (float)p->acc_y * (float)p->acc_y) );
    p->AngleZ = atan(temp)*57.3f; // 180/3.14=57.3
}

这个函数中用到了atan函数，需要在文件中包含头文件math.h

#include <math.h>

把这个计算角度的函数在qmi8658c文件中进行声明

extern void qmi8658c_fetch_angleFromAcc(t_sQMI8658C *p);

然后我们在app_main函数中调用它

void app_main(void)
{
    ESP_ERROR_CHECK(i2c_master_init());
    ESP_LOGI(TAG, "I2C initialized successfully");

    qmi8658c_init();
    
    while (1)
    {
        vTaskDelay(1000 / portTICK_PERIOD_MS);
        qmi8658c_fetch_angleFromAcc(&QMI8658C);
        ESP_LOGI(TAG, "angle_x = %.1f  angle_y = %.1f angle_y = %.1f",QMI8658C.AngleX, QMI8658C.AngleY, QMI8658C.AngleZ);
    }
}

在主函数中，qmi8658c初始化以后，每间隔1秒钟计算1次角度值，然后通过串口发送到终端，这里面把读取到的值给了QMI8658C这个变量，需要在主函数前面定义一下

t_sQMI8658C QMI8658C;

函数里面用到了freeRTOS的延时函数，需要在main.c文件的最前面包含相关头文件

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

最后，我们可以编译一下。