QMI8658A 国产六轴姿态传感器的SPI模式代码

Aov

毕设用的，写的很急，所以没啥注释。
有些功能没做完，只写了获取六轴数据，至少能用。
内置滤波器、FIFO、计步器功能啥的都没开启。
网上没啥现成的代码，以防重复造轮子。
SPI啥的直接用的逐飞库，移植的时候得删删改改点代码。
我的评价是：写的一坨
灌水

QMI86658.h

/*
 * QMI86658.h
 *
 *  Created on: 2024年4月30日
 *      Author: jason
 */

#ifndef HARDWARE_DRIVER_LAYER_QMI8658_H_
#define HARDWARE_DRIVER_LAYER_QMI8658_H_

#include "zf_driver_spi.h"
#include "zf_driver_soft_iic.h"
#include "m2_pin_common.h"
#include "error_inf.h"

#define QMI_SPI_Speed   2*1000*1000    //QMI SPI　speed should ＜ 15MHz

#define QMI_SPI_n       M2_QMI_SPI_n
#define QMI_MOSI        M2_QMI_MOSI
#define QMI_MISO        M2_QMI_MISO
#define QMI_SCK         M2_QMI_SCK
#define QMI_CS          M2_QMI_CS

#define QMI_MOSI_PIN    M2_B15
#define QMI_MISO_PIN    M2_B14
#define QMI_SCK_PIN     M2_B13
#define QMI_CS_PIN      M2_B12


#define QMI_READ_REG    0x80    //only use bit 7
#define QMI_WRITE_REG   0x00    //only use bit 7

#define QMI_reg_WHO_AM_I    0x00
#define QMI_reg_CTRL1       0x02    //set 0 1 1 0 0 0 0 0
#define QMI_reg_CTRL2       0x03    //set 0[x x x|x x x x]to set acc
#define QMI_reg_CTRL3       0x04    //set 0[x x x|x x x x]to set gyro
#define QMI_reg_CTRL5       0x06
#define QMI_reg_CTRL6       0x07
#define QMI_reg_CTRL7       0x08    //set x x x 0 x x 1 1 to enable acc and gyro
#define QMI_reg_CTRL9       0x0A

#define QMI_Temp_L          0x33
#define QMI_Temp_H          0x34

#define QMI_AX_L            0x35
#define QMI_AX_H            0x36
#define QMI_AY_L            0x37
#define QMI_AY_H            0x38
#define QMI_AZ_L            0x39
#define QMI_AZ_H            0x3A

#define QMI_QX_L            0x3B
#define QMI_QX_H            0x3C
#define QMI_QY_L            0x3D
#define QMI_QY_H            0x3E
#define QMI_QZ_L            0x3F
#define QMI_QZ_H            0x40

#define QMI_reg_RST         0x60

typedef enum{
    QMI_Set_Acceleromete_Full_Scale_2G = 0x00,
    QMI_Set_Acceleromete_Full_Scale_4G,
    QMI_Set_Acceleromete_Full_Scale_8G,
    QMI_Set_Acceleromete_Full_Scale_16G,
    QMI_Set_Acceleromete_Full_Scale_NA
}QMI_SetAccelerometeScale_enum;

typedef enum{
    QMI_Set_Gyroscope_Full_Scale_16DPS = 0x00,
    QMI_Set_Gyroscope_Full_Scale_32DPS,
    QMI_Set_Gyroscope_Full_Scale_64DPS,
    QMI_Set_Gyroscope_Full_Scale_128DPS,
    QMI_Set_Gyroscope_Full_Scale_256DPS,
    QMI_Set_Gyroscope_Full_Scale_512DPS,
    QMI_Set_Gyroscope_Full_Scale_1024PS,
    QMI_Set_Gyroscope_Full_Scale_2048DPS,
}QMI_SetGyroscopeScale_enum;

typedef enum{
    QMI_Output_Data_Rate_7174_point_4_Hz = 0x00,
    QMI_Output_Data_Rate_3587_point_2_Hz,
    QMI_Output_Data_Rate_1793_point_6_Hz,
    QMI_Output_Data_Rate_896_point_8_Hz,
    QMI_Output_Data_Rate_448_point_4_Hz,
    QMI_Output_Data_Rate_224_point_2_Hz,
    QMI_Output_Data_Rate_112_point_1_Hz,
    QMI_Output_Data_Rate_56_point_05_Hz,
    QMI_Output_Data_Rate_28_point_025_Hz,
}QMI_OutputDataRate_enum;

#define QMI_USE_AccelerometeScale   QMI_Set_Acceleromete_Full_Scale_8G
#define QMI_USE_GyroscopeScale      QMI_Set_Gyroscope_Full_Scale_2048DPS
#define QMI_USE_OutputDataRate      QMI_Output_Data_Rate_448_point_4_Hz

#define QMI_SW_AddressAutoIncrement (true)


typedef struct{
    int16  temp;
    int16  accX;
    int16  accY;
    int16  accZ;
    int16  gyroX;
    int16  gyroY;
    int16  gyroZ;

    uint8   _BUF[14];

    float  f_accX;
    float  f_accY;
    float  f_accZ;
    float  f_gyroX;
    float  f_gyroY;
    float  f_gyroZ;

    float  tolerance_accX;
    float  tolerance_accY;
    float  tolerance_accZ;
    float  tolerance_gyroX;
    float  tolerance_gyroY;
    float  tolerance_gyroZ;

}QMI_Struct;
extern QMI_Struct QMI_dat;

#define QMI_CS_LOW()    gpio_low(QMI_CS_PIN)
#define QMI_CS_HIGH()   gpio_high(QMI_CS_PIN)
void QMI8658A_SpiMode_init();
void QMI8658A_I2cMode_init();
void QMI8658A_GetData();


#endif /* HARDWARE_DRIVER_LAYER_QMI8658_H_ */

QMI86658.c

/*
 * QMI8658.c
 *
 *  Created on: 2024年4月30日
 *      Author: jason
 */
#include "QMI8658.h"


QMI_Struct QMI_dat;

const uint8 QMI_read_reg(uint8 reg)
{
    static uint8 re = 0;
    QMI_CS_LOW();
    re = spi_read_8bit_register(QMI_SPI_n, QMI_READ_REG | reg);
    QMI_CS_HIGH();
    return re;
}

const void QMI_write_reg(uint8 reg,uint8 dat)
{
    QMI_CS_LOW();
    spi_write_8bit_register(QMI_SPI_n, QMI_WRITE_REG | reg, dat);
    QMI_CS_HIGH();
}
/* ----------------------------------------------------------   *|
 * Func : QMI8658A 初始化
 * 根据QMI8658A Datasheet , SPI可以使用模式0和模式3
 * 配置寄存器 CTRL 1 2 3 7
 * CTRL 1 默认4线spi，配置开启地址自动累加
 * CTRL 2 设置加速度量程 默认配置+-8G
 * CTRL 3 设置角速度量程 默认+-2048度每秒
 * CTRL 7 设置开启加速度计与角速度计
 * ----------------------------------------------------------   */
void QMI8658A_SpiMode_init()
{
    spi_init(QMI_SPI_n ,SPI_MODE0, QMI_SPI_Speed, QMI_SCK, QMI_MOSI, QMI_MISO, QMI_CS);

    QMI_write_reg(QMI_reg_RST, 0xB0);
    system_delay_ms(500);

    while(QMI_read_reg(QMI_reg_WHO_AM_I) != 0x05)
    {
        once_error("QMI8658A_SpiMode_init : Can't read QMI WHO AM I ID 0x05 !");
    }
    QMI_write_reg(QMI_reg_CTRL9, 0xA2); //seft test
    system_delay_ms(2000);              //delay for seft test more than 1.75s


    QMI_write_reg(QMI_reg_CTRL1, 0x60); //set Address Auto Increment & Big-Endian
    QMI_write_reg(QMI_reg_CTRL2, QMI_USE_AccelerometeScale  << 4 | QMI_USE_OutputDataRate);
    QMI_write_reg(QMI_reg_CTRL3, QMI_USE_GyroscopeScale     << 4 | QMI_USE_OutputDataRate);
    QMI_write_reg(QMI_reg_CTRL5, 0x00); //滤波
    QMI_write_reg(QMI_reg_CTRL6, 0x00); //fifo
    QMI_write_reg(QMI_reg_CTRL7, 0x03); //enable

    system_delay_ms(500);
    QMI_dat.tolerance_accX = 0.0f;
    QMI_dat.tolerance_accY = 0.0f;
    QMI_dat.tolerance_accZ = 0.0f;
    QMI_dat.tolerance_gyroX = 0.0f;
    QMI_dat.tolerance_gyroY = 0.0f;
    QMI_dat.tolerance_gyroZ = 0.0f;

    //加速度和角速度的初始值矫正，需要初始化时平放。accZ为重力加速度，不做矫正。
    //后面调用QMI8658A_GetData时会用初始值减去矫正值。
    for(int16 i = 0 ; i<1000; i++)
    {
        QMI8658A_GetData();
        QMI_dat.tolerance_accX += QMI_dat.accX/1000.0f;
        QMI_dat.tolerance_accY += QMI_dat.accY/1000.0f;
        //QMI_dat.tolerance_accZ += QMI_dat.accZ/1000.0f; //no need
        QMI_dat.tolerance_gyroX += QMI_dat.gyroX/1000.0f;
        QMI_dat.tolerance_gyroY += QMI_dat.gyroY/1000.0f;
        QMI_dat.tolerance_gyroZ += QMI_dat.gyroZ/1000.0f;
    }

}


/* ----------------------------------------------------------   *|
 * 板子只设计了spi模式硬件，未对iic设计上拉电阻。
 * iic读不出来值，函数未测试。
 * MISO引脚可以配置地址，具体见datasheet
 * ----------------------------------------------------------   */
//void QMI8658A_I2cMode_init()
//{
//    gpio_init(QMI_MISO_PIN, GPO, GPIO_LOW,GPO_PUSH_PULL);
//    soft_iic_init(&iic_struct, 0x6B, 100, QMI_SCK_PIN , QMI_MOSI_PIN);
//
//}


/* ----------------------------------------------------------   *|
 * Func : QMI8658A 获取数据
 * 结构体中int16类型数据为原始数据
 * 结构体中float类型数据为矫正后的数据
 * ----------------------------------------------------------   */
void QMI8658A_GetData()
{
    QMI_CS_LOW();
    spi_read_8bit_registers(QMI_SPI_n,QMI_READ_REG | QMI_Temp_L,QMI_dat._BUF,14);
    QMI_CS_HIGH();

    QMI_dat.temp    = (int16)(((uint16)QMI_dat._BUF[1 ])<<8 | QMI_dat._BUF[0 ]);
    QMI_dat.accX    = (int16)(((uint16)QMI_dat._BUF[3 ])<<8 | QMI_dat._BUF[2 ]);
    QMI_dat.accY    = (int16)(((uint16)QMI_dat._BUF[5 ])<<8 | QMI_dat._BUF[4 ]);
    QMI_dat.accZ    = (int16)(((uint16)QMI_dat._BUF[7 ])<<8 | QMI_dat._BUF[6 ]);
    QMI_dat.gyroX   = (int16)(((uint16)QMI_dat._BUF[9 ])<<8 | QMI_dat._BUF[8 ]);
    QMI_dat.gyroY   = (int16)(((uint16)QMI_dat._BUF[11])<<8 | QMI_dat._BUF[10]);
    QMI_dat.gyroZ   = (int16)(((uint16)QMI_dat._BUF[13])<<8 | QMI_dat._BUF[12]);

    QMI_dat.f_accX = (float)QMI_dat.accX - QMI_dat.tolerance_accX;
    QMI_dat.f_accY = (float)QMI_dat.accY - QMI_dat.tolerance_accY;
    QMI_dat.f_accZ = (float)QMI_dat.accZ - QMI_dat.tolerance_accZ;
    QMI_dat.f_gyroX = (float)QMI_dat.gyroX - QMI_dat.tolerance_gyroX;
    QMI_dat.f_gyroY = (float)QMI_dat.gyroY - QMI_dat.tolerance_gyroY;
    QMI_dat.f_gyroZ = (float)QMI_dat.gyroZ - QMI_dat.tolerance_gyroZ;

}