本文档展示了如何使用keil5和J-Link对AT32A单片机进行配置，以及如何通过SPI接口初始化ICM42670加速度计和陀螺仪传感器。代码包括SPI初始化、传感器寄存器设置、数据读取和姿态解算过程。
网上其他型号代码借鉴编写来的，如果有错误，请多担待，并请指出错误，谢谢指导。
AT32A单片机的准备，我是keil，下载的keil5包
利用的j-link烧录。
下面是程序
icm42670.c
#include "ICM42670.h"
static float accSensitivity = 0.244f; //加速度的最小分辨率 mg/LSB
static float gyroSensitivity = 32.8f;
extern signed short ax;
extern signed short ay;
extern signed short az;
extern signed short gx;
extern signed short gy;
extern signed short gz;
void spi1_init(void)
{
gpio_init_type gpio_init_struct;
spi_init_type spi_init_struct;
crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE);
crm_periph_clock_enable(CRM_SPI1_PERIPH_CLOCK, TRUE);
gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; //cs
gpio_init_struct.gpio_pull = GPIO_PULL_UP;
gpio_init_struct.gpio_mode = GPIO_MODE_OUTPUT;
gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
gpio_init_struct.gpio_pins = GPIO_PINS_4;
gpio_init(GPIOA, &gpio_init_struct);
//gpio_bits_set(GPIOA,GPIO_PINS_4);
gpio_pin_mux_config(GPIOA, GPIO_PINS_SOURCE5, GPIO_MUX_0); //这个muxing 百度翻译也整不明白，是不是管角复用的意思？看到数据手册是乎是5的第1功能为SPI1
gpio_pin_mux_config(GPIOA, GPIO_PINS_SOURCE6, GPIO_MUX_0);
gpio_pin_mux_config(GPIOA, GPIO_PINS_SOURCE7, GPIO_MUX_0);
gpio_default_para_init(&gpio_init_struct);
gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL;
gpio_init_struct.gpio_pull = GPIO_PULL_DOWN;
gpio_init_struct.gpio_mode = GPIO_MODE_MUX;
gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
gpio_init_struct.gpio_pins = GPIO_PINS_5;
gpio_init(GPIOA, &gpio_init_struct);
gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL;
gpio_init_struct.gpio_pull = GPIO_PULL_UP;
gpio_init_struct.gpio_mode = GPIO_MODE_MUX;
gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
gpio_init_struct.gpio_pins = GPIO_PINS_6 | GPIO_PINS_7;
gpio_init(GPIOA, &gpio_init_struct);
spi_default_para_init(&spi_init_struct);
spi_init_struct.transmission_mode = SPI_TRANSMIT_FULL_DUPLEX;
spi_init_struct.master_slave_mode = SPI_MODE_MASTER;
spi_init_struct.mclk_freq_division = SPI_MCLK_DIV_8;
spi_init_struct.first_bit_transmission = SPI_FIRST_BIT_MSB;
spi_init_struct.frame_bit_num = SPI_FRAME_8BIT;
spi_init_struct.clock_polarity = SPI_CLOCK_POLARITY_HIGH;
spi_init_struct.clock_phase = SPI_CLOCK_PHASE_2EDGE;
spi_init_struct.cs_mode_selection = SPI_CS_SOFTWARE_MODE;
spi_init(SPI1, &spi_init_struct);
spi_enable(SPI1, TRUE);
}
uint8_t spi1_readwritebyte(uint8_t dat)
{
while (spi_i2s_flag_get(SPI1, SPI_I2S_TDBE_FLAG) == RESET);
spi_i2s_data_transmit(SPI1, dat);
while (spi_i2s_flag_get(SPI1, SPI_I2S_RDBF_FLAG) == RESET);
return spi_i2s_data_receive(SPI1);
}
//uint8_t icm42670_readwritebyte(uint8_t dat)
//{
// return spi1_readwritebyte(dat);
//}
void spi_receice(uint8_t* pdata, uint16_t len)
{
uint16_t i = 0;
for( i = 0; i < len; i ++)
{
pdata[i] = spi1_readwritebyte(0);
// pdata++;
//spi1_readwritebyte(pdata[i]);
}
}
void ICM42670_Init(void)
{
uint8_t reg_val = 0;
reg_val = icm42670_read_reg(ICM42670_WHO_AM_I);
icm42670_write_reg(ICM42670_PWR_MGMT0,0x00);
delay_sec(1);
icm42670_write_reg(ICM42670_DEVICE_CONFIG, 0x04);//4线spi
icm42670_write_reg(ICM42670_FIFO_CONFIG1, 0x00);//Stream-to-FIFO Mode
reg_val = icm42670_read_reg(ICM42670_INT_SOURCE0);
icm42670_write_reg(ICM42670_INT_SOURCE0, 0x00);
icm42670_write_reg(ICM42670_FIFO_CONFIG2, 0x00); // watermark
icm42670_write_reg(ICM42670_FIFO_CONFIG3, 0x02); // watermark
icm42670_write_reg(ICM42670_INT_SOURCE0, reg_val);
icm42670_write_reg(ICM42670_FIFO_CONFIG1, 0x33); // Enable the accel and gyro to the FIFO
icm42670_write_reg(ICM42670_INT_CONFIG, 0x36);
reg_val = icm42670_read_reg(ICM42670_INT_SOURCE0);
reg_val |= (1 << 2); //FIFO_THS_INT1_ENABLE
icm42670_write_reg(ICM42670_INT_SOURCE0, reg_val);
bsp_Icm42670GetAres(AFS_8G);
reg_val = icm42670_read_reg(ICM42670_ACCEL_CONFIG0);//page74
reg_val |= (AFS_8G << 5); //量程 ±8g
reg_val |= (AODR_50Hz); //输出速率 50HZ
icm42670_write_reg(ICM42670_ACCEL_CONFIG0, reg_val);
bsp_Icm42670GetGres(GFS_1000DPS);
reg_val = icm42670_read_reg(ICM42670_GYRO_CONFIG0);
//page73
reg_val |= (GFS_1000DPS << 5); //量程 ±1000dps
reg_val |= (GODR_50Hz); //输出速率 50HZ
icm42670_write_reg(ICM42670_GYRO_CONFIG0, reg_val);
reg_val = icm42670_read_reg(ICM42670_PWR_MGMT0); //读取PWR—MGMT0当前寄存器的值(page72)
//reg_val &= ~(1 << 5);//使能温度测量
reg_val |= ((3) << 2);//设置GYRO_MODE 0:关闭 1:待机 2:预留 3:低噪声
reg_val |= (3);//设置ACCEL_MODE 0:关闭 1:关闭 2:低功耗 3:低噪声
icm42670_write_reg(ICM42670_PWR_MGMT0, reg_val);
delay_sec(1); //操作完PWR—MGMT0寄存器后 200us内不能有任何读写寄存器的操作
// return 0;
}
uint8_t icm42670_read_reg(uint8_t reg)
{
uint8_t regval = 0;
ICM_SPI_CS_LOW();
reg |= 0x80;
spi1_readwritebyte(reg);
//spi1_readwritebyte(®val,1);
spi_receice(®val,1);
ICM_SPI_CS_HIGH();
//gpio_bits_set(GPIOA,GPIO_PINS_4);
return regval;
}
uint8_t icm42670_read_regs(uint8_t reg, uint8_t* buf, uint16_t len)
{
reg |= 0x80;
ICM_SPI_CS_LOW();
/* 写入要读的寄存器地址 */
spi1_readwritebyte(reg);
/* 读取寄存器数据 */
spi_receice(buf,len);
ICM_SPI_CS_HIGH();
}
static uint8_t icm42670_write_reg(uint8_t reg, uint8_t value)
{
uint8_t status;
ICM_SPI_CS_LOW();
status = spi1_readwritebyte(reg);
spi1_readwritebyte(value);
//spi_receice(&value, 1);
ICM_SPI_CS_HIGH();
return status;
//return 0;
}
int8_t bsp_IcmGetAccelerometer()
{
uint8_t buffer[6] = {0};
icm42670_read_regs(ICM42670_ACCEL_XOUT_H, buffer, 6);
ax = ((uint16_t)buffer[0] << 8) | buffer[1];
ay = ((uint16_t)buffer[2] << 8) | buffer[3];
az = ((uint16_t)buffer[4] << 8) | buffer[5];
ax = (int16_t)(ax * accSensitivity);
ay = (int16_t)(ay * accSensitivity);
az = (int16_t)(az * accSensitivity);
return 0;
}
int8_t bsp_IcmGetGyroscope()
{
uint8_t buffer[6] = {0};
icm42670_read_regs(ICM42670_GYRO_XOUT_H, buffer, 6);
gx = ((uint16_t)buffer[0] << 8) | buffer[1];
gy = ((uint16_t)buffer[2] << 8) | buffer[3];
gz = ((uint16_t)buffer[4] << 8) | buffer[5];
gx = (int16_t)(gx * gyroSensitivity);
gy = (int16_t)(gy * gyroSensitivity);
gz = (int16_t)(gz * gyroSensitivity);
return 0;
}
int8_t bsp_IcmGetRawData()
{
uint8_t buffer[12] = {0};
icm42670_read_regs(ICM42670_ACCEL_XOUT_H, buffer, 12);
ax = ((uint16_t)buffer[0] << 8) | buffer[1];
ay = ((uint16_t)buffer[2] << 8) | buffer[3];
az = ((uint16_t)buffer[4] << 8) | buffer[5];
gx = ((uint16_t)buffer[6] << 8) | buffer[7];
gy = ((uint16_t)buffer[8] << 8) | buffer[9];
gz = ((uint16_t)buffer[10] << 8) | buffer[11];
ax = (int16_t)(ax * accSensitivity);
ay = (int16_t)(ay * accSensitivity);
az = (int16_t)(az * accSensitivity);
gx = (int16_t)(gx * gyroSensitivity);
gy = (int16_t)(gy * gyroSensitivity);
gz = (int16_t)(gz * gyroSensitivity);
return 0;
}
//int8_t bsp_IcmGetRawData(icm42670RawData_t* accData, icm42670RawData_t* GyroData)
//{
// uint8_t buffer[12] = {0};
// icm42670_read_regs(ICM42670_ACCEL_XOUT_H, buffer, 12);
// ax = accData->x = ((uint16_t)buffer[0] << 8) | buffer[1];
// ay = accData->y = ((uint16_t)buffer[2] << 8) | buffer[3];
// az = accData->z = ((uint16_t)buffer[4] << 8) | buffer[5];
// gx = GyroData->x = ((uint16_t)buffer[6] << 8) | buffer[7];
// gy = GyroData->y = ((uint16_t)buffer[8] << 8) | buffer[9];
// gz = GyroData->z = ((uint16_t)buffer[10] << 8) | buffer[11];
// ax = accData->x = (int16_t)(accData->x * accSensitivity);
// ay = accData->y = (int16_t)(accData->y * accSensitivity);
// az = accData->z = (int16_t)(accData->z * accSensitivity);
// gx = GyroData->x = (int16_t)(GyroData->x * gyroSensitivity);
// gy = GyroData->y = (int16_t)(GyroData->y * gyroSensitivity);
// gz = GyroData->z = (int16_t)(GyroData->z * gyroSensitivity);
// return 0;
//}
float bsp_Icm42670GetAres(uint8_t Ascale)
{
switch(Ascale)
{
// Possible accelerometer scales (and their register bit settings) are:
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
case AFS_2G:
accSensitivity = 2000 / 32768.0f;
break;
case AFS_4G:
accSensitivity = 4000 / 32768.0f;
break;
case AFS_8G:
accSensitivity = 8000 / 32768.0f;
break;
case AFS_16G:
accSensitivity = 16000 / 32768.0f;
break;
}
return accSensitivity;
}
float bsp_Icm42670GetGres(uint8_t Gscale)
{
switch(Gscale)
{
case GFS_250DPS:
gyroSensitivity = 250.0f / 32768.0f;
break;
case GFS_500DPS:
gyroSensitivity = 500.0f / 32768.0f;
break;
case GFS_1000DPS:
gyroSensitivity = 1000.0f / 32768.0f;
break;
case GFS_2000DPS:
gyroSensitivity = 2000.0f / 32768.0f;
break;
}
return gyroSensitivity;
}
icm42670.h
#ifndef __ICM42670_H__
#define __ICM42670_H__
#include <stdint.h>
#include "stdio.h"
#include "at32f421.h"
#define ICM42670_DEVICE_CONFIG 0x01
#define ICM42670_DRIVE_CONFIG1 0x03
#define ICM42670_DRIVE_CONFIG2 0x04
#define ICM42670_DRIVE_CONFIG3 0x05
#define ICM42670_INT_CONFIG 0x06
#define ICM42670_FIFO_CONFIG1 0x28
#define ICM42670_FIFO_CONFIG2 0x29
#define ICM42670_FIFO_CONFIG3 0x2A
//#define ICM42670_TEMP_DATA1 0x09
//#define ICM42670_TEMP_DATA0 0x0A
#define ICM42670_SIGNAL_PATH_RESET 0x02
#define ICM42670_INT_SOURCE0 0x2B
#define ICM42670_INT_SOURCE1 0x2C
#define ICM42670_INT_SOURCE3 0x2D
#define ICM42670_INT_SOURCE4 0x2E
#define ICM42670_WHO_AM_I 0x75
#define AFS_2G 0x03
#define AFS_4G 0x02
#define AFS_8G 0x01
#define AFS_16G 0x00 // default
#define GFS_2000DPS 0x00 // default
#define GFS_1000DPS 0x01
#define GFS_500DPS 0x02
#define GFS_250DPS 0x03
#define AODR_1600Hz 0x05
#define AODR_800Hz 0x06 // default
#define AODR_400Hz 0x07
#define AODR_200Hz 0x08
#define AODR_100Hz 0x09
#define AODR_50Hz 0x0A
#define AODR_25Hz 0x0B
#define AODR_12_5Hz 0x0C
#define AODR_6_25Hz 0x0D
#define AODR_3_125Hz 0x0E
#define AODR_1_5625Hz 0x0F
#define GODR_1600Hz 0x05
#define GODR_800Hz 0x06 // default
#define GODR_400Hz 0x07
#define GODR_200Hz 0x08
#define GODR_100Hz 0x09
#define GODR_50Hz 0x0A
#define GODR_25Hz 0x0B
#define GODR_12_5Hz 0x0C
#define ICM42670_GYRO_CONFIG0 0x20
#define ICM42670_ACCEL_CONFIG0 0x21
#define ICM42670_GYRO_CONFIG1 0x23
//#define ICM42670_GYRO_ACCEL_CONFIG0 0x52
#define ICM42670_ACCEL_CONFIG1 0x24
#define DEVICE_CONFIG 0x01
#define DRIVE_CONFIG3 0x05
#define ICM42670_ACCEL_XOUT_H 0x0B
#define ICM42670_ACCEL_XOUT_L 0x0C
#define ICM42670_ACCEL_YOUT_H 0x0D
#define ICM42670_ACCEL_YOUT_L 0x0E
#define ICM42670_ACCEL_ZOUT_H 0x0F
#define ICM42670_ACCEL_ZOUT_L 0x10
#define ICM42670_GYRO_XOUT_H 0x11
#define ICM42670_GYRO_XOUT_L 0x12
#define ICM42670_GYRO_YOUT_H 0x13
#define ICM42670_GYRO_YOUT_L 0x14
#define ICM42670_GYRO_ZOUT_H 0x15
#define ICM42670_GYRO_ZOUT_L 0x16
//#define SELF_TEST_CONFIG 0X70
#define ICM42670_PWR_MGMT0 0x1F //电源管理，典型值：0x00(正常启用)
#define ICM_SPI_CS_LOW() gpio_bits_reset(GPIOA, GPIO_PINS_4)
#define ICM_SPI_CS_HIGH() gpio_bits_set(GPIOA, GPIO_PINS_4)
typedef struct {
int16_t x; /**< Raw int16_t value from the x axis */
int16_t y; /**< Raw int16_t value from the y axis */
int16_t z; /**< Raw int16_t value from the z axis */
} icm42670RawData_t;
float bsp_Icm42670GetAres(uint8_t Ascale);
float bsp_Icm42670GetGres(uint8_t Gscale);
//void GPIO_Init(void);
void spi1_init(void);
uint8_t spi1_readwritebyte(uint8_t dat);
//uint8_t icm42670_readwritebyte(uint8_t dat);
void ICM42670_Init(void);
uint8_t icm42670_read_reg(uint8_t reg);
uint8_t icm42670_read_regs(uint8_t reg, uint8_t* buf, uint16_t len);
static uint8_t icm42670_write_reg(uint8_t reg, uint8_t value);
int8_t bsp_IcmGetAccelerometer();
int8_t bsp_IcmGetGyroscope();
int8_t bsp_IcmGetRawData();
//int8_t bsp_IcmGetRawData(icm42670RawData_t *accData,icm42670RawData_t *GyroData);
#endif姿态角代码
icmupdate.c
#include <math.h>
#define Kp 10.0f // 这里的KpKi是用于调整加速度计修正陀螺仪的速度
#define Ki 0.008f
#define halfT 0.05f // 采样周期的一半，用于求解四元数微分方程时计算角增量
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 初始位置姿态角为：0、0、0，对应四元数为：1、0、0、0
float exInt = 0, eyInt = 0, ezInt = 0;
//重力加速度在三轴上的分量与用当前姿态计算得来的重力在三轴上的分量的误差的积分
float Q_ANGLE_X= 0, Q_ANGLE_Y = 0, Q_ANGLE_Z = 0;
//互补滤波函数
//输入参数：g表陀螺仪角速度(弧度/s)，a表加计（m/s2或g都可以，会归一化）
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float q0temp,q1temp,q2temp,q3temp;//四元数暂存变量，求解微分方程时要用
float norm; //矢量的模或四元数的范数
float vx, vy, vz;//当前姿态计算得来的重力在三轴上的分量
float ex, ey, ez;//当前加计测得的重力加速度在三轴上的分量
//与用当前姿态计算得来的重力在三轴上的分量的误差
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q1q1 = q1*q1;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
if(ax*ay*az==0)//加计处于自由落体状态时不进行姿态解算，因为会产生分母无穷大的情况
return;
norm = sqrt(ax*ax + ay*ay + az*az);//单位化加速度计，
ax = ax / norm;// 这样变更了量程也不需要修改KP参数，因为这里归一化了
ay = ay / norm;
az = az / norm;
//用当前姿态计算出重力在三个轴上的分量，重力在n系下是[0,0,g]，乘以转换矩阵就转到b系
//参考坐标n系转化到载体坐标b系，用四元数表示的方向余弦矩阵第三行即是
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
//计算测得的重力与计算得重力间的误差，这个误差是通过向量外积（也就是叉乘）求出来的
ex = (ay*vz - az*vy) ;
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;
exInt = exInt + ex * Ki; //对误差进行积分
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
gx = gx + Kp*ex + exInt; //将误差PI（比例和积分项）补偿到陀螺仪角速度
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt; //没有磁力计的话就无法修正偏航角
//下面进行姿态的更新，也就是四元数微分方程的求解
q0temp=q0;
q1temp=q1;
q2temp=q2;
q3temp=q3;
//采用一阶毕卡解法，相关知识可参见《惯性器件与惯性导航系统》P212
q0 = q0temp + (-q1temp*gx - q2temp*gy -q3temp*gz)*halfT;
q1 = q1temp + (q0temp*gx + q2temp*gz -q3temp*gy)*halfT;
q2 = q2temp + (q0temp*gy - q1temp*gz +q3temp*gx)*halfT;
q3 = q3temp + (q0temp*gz + q1temp*gy -q2temp*gx)*halfT;
//单位化四元数在空间旋转时不会拉伸，仅有旋转角度，这类似线性代数里的正交变换
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//四元数到欧拉角的转换
//其中YAW航向角由于加速度计对其没有修正作用，因此此处直接用陀螺仪积分代替
Q_ANGLE_Z = Q_ANGLE_Z + gz*halfT*2*57.3; // yaw
//Q_ANGLE_Z = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
Q_ANGLE_Y = asin(-2 * q1 * q3 + 2 * q0* q2)*57.3; // pitch
Q_ANGLE_X = atan2(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}mian.c
int main(void)
{
spi1_init();
//system_clock_config();
at32_board_init();
uart_print_init(9600);
ICM42670_Init();
/* output a message on hyperterminal using printf function */
while(1)
{
//printf("usart printf counter: %x\r\n", icm42670_read_reg(0x75));
bsp_IcmGetAccelerometer();
printf("2.AccX:%d-AccY:%d-AccZ:%d\r\n",ax,ay,az);
bsp_IcmGetAccelerometer();
printf("3.GyroX:%d-GyroY:%d-GyroZ:%d\r\n",gx,gy,gz);
bsp_IcmGetRawData();
printf("4.AccX:%d-AccY:%d-AccZ:%d-GyroX:%d-GyroY:%d-GyroZ:%d\r\n",ax,ay,az,gx,gy,gz);
//bsp_IcmGetRawData(&stAccData,&stGyroData);
//printf("4.AccX:%d-AccY:%d-AccZ:%d-GyroX:%d-GyroY:%d-GyroZ:%d\r\n",stAccData.x,stAccData.y,stAccData.z,stGyroData.x,stGyroData.y,stGyroData.z);
IMUupdate((float)gx*500/57.3/65536, (float)gy*500/57.3/65536, (float)gz*500/57.3/65536,
(float)ax*9.8/16384, (float)ay*9.8/16384, (float)az*9.8/16384);
printf("pitch:%.2f roll:%.2f yaw:%.2f\r\n",Q_ANGLE_X,Q_ANGLE_Y,Q_ANGLE_Z);
delay_sec(1);
}
}串口显示
完整代码连接
扫一扫
3619
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
