/********************************************************************************************************************* * RT1064DVL6A Opensourec Library 即（RT1064DVL6A 开源库）是一个基于官方 SDK 接口的第三方开源库 * Copyright (c) 2022 SEEKFREE 逐飞科技 * * 本文件是 RT1064DVL6A 开源库的一部分 * * RT1064DVL6A 开源库 是免费软件 * 您可以根据自由软件基金会发布的 GPL（GNU General Public License，即 GNU通用公共许可证）的条款 * 即 GPL 的第3版（即 GPL3.0）或（您选择的）任何后来的版本，重新发布和/或修改它 * * 本开源库的发布是希望它能发挥作用，但并未对其作任何的保证 * 甚至没有隐含的适销性或适合特定用途的保证 * 更多细节请参见 GPL * * 您应该在收到本开源库的同时收到一份 GPL 的副本 * 如果没有，请参阅 * * 额外注明： * 本开源库使用 GPL3.0 开源许可证协议 以上许可申明为译文版本 * 许可申明英文版在 libraries/doc 文件夹下的 GPL3_permission_statement.txt 文件中 * 许可证副本在 libraries 文件夹下 即该文件夹下的 LICENSE 文件 * 欢迎各位使用并传播本程序 但修改内容时必须保留逐飞科技的版权声明（即本声明） * * 文件名称 zf_device_imu660rc * 公司名称 成都逐飞科技有限公司 * 版本信息 查看 libraries/doc 文件夹内 version 文件 版本说明 * 开发环境 ADS v1.10.2 * 适用平台 TC264 * 店铺链接 https://seekfree.taobao.com/ * * 修改记录 * 日期 作者 备注 * 2025-12-12 SeekFree first version ********************************************************************************************************************/ /********************************************************************************************************************* * 接线定义： * ------------------------------------ * 模块管脚 单片机管脚 * // 硬件 SPI 引脚 * SCL/SPC 查看 zf_device_imu660rc.h 中 IMU660RC_SPC_PIN 宏定义 * SDA/DSI 查看 zf_device_imu660rc.h 中 IMU660RC_SDI_PIN 宏定义 * SA0/SDO 查看 zf_device_imu660rc.h 中 IMU660RC_SDO_PIN 宏定义 * CS 查看 zf_device_imu660rc.h 中 IMU660RC_CS_PIN 宏定义 * INT2 查看 zf_device_imu660rc.h 中 IMU660RC_INT2_PIN 宏定义 * VCC 3.3V电源 * GND 电源地 * 其余引脚悬空 * * // 软件 IIC 引脚 * SCL/SPC 查看 zf_device_imu660rc.h 中 IMU660RC_SCL_PIN 宏定义 * SDA/DSI 查看 zf_device_imu660rc.h 中 IMU660RC_SDA_PIN 宏定义 * VCC 3.3V电源 * GND 电源地 * 其余引脚悬空 * ------------------------------------ ********************************************************************************************************************/ #include "math.h" #include "zf_common_debug.h" #include "zf_driver_delay.h" #include "zf_driver_spi.h" #include "zf_driver_gpio.h" #include "zf_driver_soft_iic.h" #include "zf_device_config.h" #include "zf_driver_uart.h" #include "zf_driver_exti.h" #include "zf_device_imu660rc.h" #ifndef M_PI #define M_PI 3.1415926f #endif static uint8 imu660rc_quarternion_rate; float imu660rc_transition_factor[2]; int16 imu660rc_gyro_x = 0, imu660rc_gyro_y = 0, imu660rc_gyro_z = 0; // 三轴陀螺仪数据 gyro (陀螺仪) int16 imu660rc_acc_x = 0, imu660rc_acc_y = 0, imu660rc_acc_z = 0; // 三轴加速度计数据 acc (accelerometer 加速度计) float imu660rc_roll = 0, imu660rc_pitch = 0, imu660rc_yaw = 0; // 欧拉角 float imu660rc_quarternion[4]; // 四元数 #if IMU660RC_USE_SOFT_IIC static soft_iic_info_struct imu660rc_iic_struct; #define imu660rc_write_register(reg, data) (soft_iic_write_8bit_register (&imu660rc_iic_struct, (reg), (data))) #define imu660rc_write_registers(reg, data, len) (soft_iic_write_8bit_registers(&imu660rc_iic_struct, (reg), (data), (len))) #define imu660rc_read_register(reg) (soft_iic_read_8bit_register (&imu660rc_iic_struct, (reg))) #define imu660rc_read_registers(reg, data, len) (soft_iic_read_8bit_registers (&imu660rc_iic_struct, (reg), (data), (len))) #else //------------------------------------------------------------------------------------------------------------------- // 函数简介 IMU660RC 写寄存器 // 参数说明 reg 寄存器地址 // 参数说明 data 数据 // 返回参数 void // 使用示例 // 备注信息 内部调用 //------------------------------------------------------------------------------------------------------------------- static void imu660rc_write_register (uint8 reg, uint8 data) { IMU660RC_CS(0); spi_write_8bit_register(IMU660RC_SPI, reg | IMU660RC_SPI_W, data); IMU660RC_CS(1); } //------------------------------------------------------------------------------------------------------------------- // 函数简介 IMU660RC 读寄存器 // 参数说明 reg 寄存器地址 // 返回参数 uint8 数据 // 使用示例 imu660rc_read_register(IMU660RC_CHIP_ID); // 备注信息 内部调用 //------------------------------------------------------------------------------------------------------------------- static uint8 imu660rc_read_register (uint8 reg) { uint8 data; IMU660RC_CS(0); data = spi_read_8bit_register(IMU660RC_SPI, reg | IMU660RC_SPI_R); IMU660RC_CS(1); return data; } //------------------------------------------------------------------------------------------------------------------- // 函数简介 IMU660RC 读数据 // 参数说明 reg 寄存器地址 // 参数说明 data 数据缓冲区 // 参数说明 len 数据长度 // 返回参数 void // 使用示例 imu660rc_read_registers(IMU660RC_ACC_ADDRESS, dat, 6); // 备注信息 内部调用 //------------------------------------------------------------------------------------------------------------------- static void imu660rc_read_registers (uint8 reg, uint8 *data, uint32 len) { IMU660RC_CS(0); spi_read_8bit_registers(IMU660RC_SPI, reg | IMU660RC_SPI_R, data, len); IMU660RC_CS(1); } #endif //------------------------------------------------------------------------------------------------------------------- // 函数简介 IMU660RC fp16转浮点数 // 参数说明 void // 返回参数 uint8 // 使用示例 // 备注信息 内部调用 //------------------------------------------------------------------------------------------------------------------- static uint32 fp16_to_float(uint16 h) { uint16 h_exp = (h & 0x7c00u); uint32 f_sgn = ((uint32)h & 0x8000u) << 16; switch (h_exp) { case 0x0000u: // 0 or subnormal { uint16 h_sig = (h & 0x03ffu); // Signed zero if (h_sig == 0) { return f_sgn; } // Subnormal h_sig <<= 1; while ((h_sig & 0x0400u) == 0) { h_sig <<= 1; h_exp++; } uint32 f_exp = ((uint32)(127 - 15 - h_exp)) << 23; uint32 f_sig = ((uint32)(h_sig & 0x03ffu)) << 13; return f_sgn + f_exp + f_sig; } case 0x7c00u: // inf or NaN { // All-ones exponent and a copy of the significand return f_sgn + 0x7f800000u + (((uint32)(h & 0x03ffu)) << 13); } default: // normalized { // Just need to adjust the exponent and shift return f_sgn + (((uint32)(h & 0x7fffu) + 0x1c000u) << 13); } } } //------------------------------------------------------------------------------------------------------------------- // 函数简介 IMU660RC 四元数归一化 // 参数说明 void // 返回参数 uint8 // 使用示例 // 备注信息 内部调用 //------------------------------------------------------------------------------------------------------------------- static void quarternion_normalize(float quat[4], uint16 *fp16) { float n = 0; float temp[4]; *(uint32 *)(&temp[0]) = fp16_to_float(fp16[0]); *(uint32 *)(&temp[1]) = fp16_to_float(fp16[1]); *(uint32 *)(&temp[2]) = fp16_to_float(fp16[2]); *(uint32 *)(&temp[3]) = fp16_to_float(fp16[3]); n = temp[0] * temp[0] + temp[1] * temp[1] + temp[2] * temp[2] + temp[3] * temp[3]; n = sqrtf(n); if(n > 0.001f) // 避免除以接近0的值 { n = temp[3] < 0.0f ? -n : n; quat[0] = temp[1] / n; quat[1] = temp[2] / n; quat[2] = temp[0] / n; quat[3] = temp[3] / n; } } //------------------------------------------------------------------------------------------------------------------- // 函数简介 IMU660RC 四元数转欧拉角 // 参数说明 void // 返回参数 uint8 // 使用示例 // 备注信息 内部调用 //------------------------------------------------------------------------------------------------------------------- static void quarternion_to_euler(float quat[4], float *roll, float *pitch, float *yaw) { float euler[3]; float sqx = quat[0] * quat[0]; float sqy = quat[1] * quat[1]; float sqz = quat[2] * quat[2]; euler[0] = atan2f(2.0f * (quat[1] * quat[3] + quat[0] * quat[2]), 1.0f - 2.0f * (sqy + sqx)); euler[1] = -asinf(2.0f * (quat[0] * quat[3] - quat[1] * quat[2])); euler[2] = atan2f(2.0f * (quat[0] * quat[1] + quat[2] * quat[3]), 1.0f - 2.0f * (sqx + sqz)); // 弧度转角度 euler[0] = 180 * (euler[0]) / M_PI; euler[1] = 180 * (euler[1]) / M_PI; euler[2] = 180 * (euler[2]) / M_PI; // 角度调整 euler[2] = 0 > euler[2] ? euler[2] + 360 : euler[2]; *roll = euler[0]; *pitch = euler[1]; *yaw = euler[2]; } //------------------------------------------------------------------------------------------------------------------- // 函数简介 IMU660RC 设置访问区域 // 参数说明 void // 返回参数 uint8 // 使用示例 // 备注信息 内部调用 //------------------------------------------------------------------------------------------------------------------- static void imu660rc_set_mem_bank (imu660rc_mem_bank_enum bank) { imu660rc_write_register(IMU660RC_FUNC_CFG_ACCESS, bank); } //------------------------------------------------------------------------------------------------------------------- // 函数简介 IMU660RC 自检 // 参数说明 void // 返回参数 uint8 1-自检失败 0-自检成功 // 使用示例 imu660rc_self_check(); // 备注信息 内部调用 //------------------------------------------------------------------------------------------------------------------- static uint8 imu660rc_self_check (void) { uint8 dat = 0, return_state = 0; uint16 timeout_count = 0; do { if(IMU660RC_TIMEOUT_COUNT < timeout_count ++) { return_state = 1; break; } dat = imu660rc_read_register(IMU660RC_CHIP_ID); system_delay_ms(1); }while(0x70 != dat); // 读取设备ID是否等于0x70，如果不是0x70则认为没检测到设备 return return_state; } //------------------------------------------------------------------------------------------------------------------- // 函数简介 获取 IMU660RC 加速度计数据 // 参数说明 void // 返回参数 void // 使用示例 imu660rc_get_acc(); // 执行该函数后，直接查看对应的变量即可 // 备注信息 使用 SPI 的采集时间为10us //------------------------------------------------------------------------------------------------------------------- void imu660rc_get_acc (void) { int16 dat[3]; if(IMU660RC_QUARTERNION_DISABLE == imu660rc_quarternion_rate) { // 仅在四元数关闭状态下支持调用此函数， 四元数开启时如果需要读取加速度信息则IMU660RC_QUARTERNION_GET_ACC设置为1 imu660rc_read_registers(IMU660RC_OUTX_L_A, (uint8 *)dat, 6); imu660rc_acc_x = dat[0]; imu660rc_acc_y = dat[1]; imu660rc_acc_z = dat[2]; } } //------------------------------------------------------------------------------------------------------------------- // 函数简介 获取 IMU660RC 陀螺仪数据 // 参数说明 void // 返回参数 void // 使用示例 imu660rc_get_gyro(); // 执行该函数后，直接查看对应的变量即可 // 备注信息 使用 SPI 的采集时间为10us //------------------------------------------------------------------------------------------------------------------- void imu660rc_get_gyro (void) { int16 dat[3]; if(IMU660RC_QUARTERNION_DISABLE == imu660rc_quarternion_rate) { // 仅在四元数关闭状态下支持调用此函数， 四元数开启时如果需要读取加速度信息则IMU660RC_QUARTERNION_GET_GYRO设置为1 imu660rc_read_registers(IMU660RC_OUTX_L_G, (uint8 *)dat, 6); imu660rc_gyro_x = dat[0]; imu660rc_gyro_y = dat[1]; imu660rc_gyro_z = dat[2]; } } //------------------------------------------------------------------------------------------------------------------- // 函数简介 获取 IMU660RC 四元数数据并转换为欧拉角 // 参数说明 void // 返回参数 void // 使用示例 imu660rc_get_quarternion(); // 执行该函数后，直接查看对应的变量即可 // 备注信息 需要在INT2引脚触发的上升沿中断调用此函数 // //------------------------------------------------------------------------------------------------------------------- void imu660rc_get_quarternion(void) { uint8 i; uint16 buff[4]; uint8 *buff1_ptr; int16 *buff2_ptr; if(IMU660RC_QUARTERNION_DISABLE != imu660rc_quarternion_rate) { buff1_ptr = (uint8 *)buff; imu660rc_set_mem_bank(IMU660RC_EMBED_MEM_BANK); imu660rc_write_register(IMU660RC_PAGE_RW, 0x20); imu660rc_write_register(IMU660RC_PAGE_SEL, 0x31); for(i = 0; 8 > i; i++) { imu660rc_write_register(0x08, 0x4C + i); buff1_ptr[i] = imu660rc_read_register(0x09); } imu660rc_write_register(IMU660RC_PAGE_RW, 0x0); imu660rc_set_mem_bank(IMU660RC_MAIN_MEM_BANK); // 四元数归一化 quarternion_normalize(imu660rc_quarternion, buff); // 四元数转欧拉角 quarternion_to_euler(imu660rc_quarternion, &imu660rc_roll, &imu660rc_pitch, &imu660rc_yaw); // 读取加速度与角速度信息 #if(1 == IMU660RC_QUARTERNION_GET_ACC) { buff2_ptr = (int16 *)buff; imu660rc_read_registers(IMU660RC_OUTX_L_A, (uint8 *)buff2_ptr, 6); imu660rc_acc_x = buff2_ptr[0]; imu660rc_acc_y = buff2_ptr[1]; imu660rc_acc_z = buff2_ptr[2]; } #endif #if(1 == IMU660RC_QUARTERNION_GET_GYRO) { buff2_ptr = (int16 *)buff; imu660rc_read_registers(IMU660RC_OUTX_L_G, (uint8 *)buff2_ptr, 6); imu660rc_gyro_x = buff2_ptr[0]; imu660rc_gyro_y = buff2_ptr[1]; imu660rc_gyro_z = buff2_ptr[2]; } #endif } } //------------------------------------------------------------------------------------------------------------------- // 函数简介 IMU660RC中断回调函数，主要在中断内读取四元数 // 参数说明 void // 返回参数 void // 使用示例 // 备注信息 // //------------------------------------------------------------------------------------------------------------------- void imu660rc_callback(void) { imu660rc_get_quarternion(); } //------------------------------------------------------------------------------------------------------------------- // 函数简介 初始化 IMU660RC // 参数说明 quarternion_rate 选择四元数输出速率 // 返回参数 uint8 1-初始化失败 0-初始化成功 // 使用示例 imu660rc_init(IMU660RC_QUARTERNION_120HZ); // 备注信息 //------------------------------------------------------------------------------------------------------------------- uint8 imu660rc_init(imu660rc_quarternion_rate_config quarternion_rate) { uint8 return_state = 0; imu660rc_quarternion_rate = quarternion_rate; #if IMU660RC_USE_SOFT_IIC soft_iic_init(&imu660rc_iic_struct, IMU660RC_DEV_ADDR, IMU660RC_SOFT_IIC_DELAY, IMU660RC_SCL_PIN, IMU660RC_SDA_PIN); // 配置 IMU660RC 的 IIC 端口 #else spi_init(IMU660RC_SPI, SPI_MODE0, IMU660RC_SPI_SPEED, IMU660RC_SPC_PIN, IMU660RC_SDI_PIN, IMU660RC_SDO_PIN, SPI_CS_NULL); // 配置 IMU660RC 的 SPI 端口 gpio_init(IMU660RC_CS_PIN, GPO, GPIO_HIGH, GPO_PUSH_PULL); // 配置 IMU660RC 的CS端口 #endif system_delay_ms(10); do { if(imu660rc_self_check()) // IMU660RC 自检 { // 如果程序在输出了断言信息 并且提示出错位置在这里 // 那么就是 IMU660RC 自检出错并超时退出了 // 检查一下接线有没有问题 如果没问题可能就是坏了 zf_log(0, "imu660rc self check error."); return_state = 1; break; } // 复位传感器 imu660rc_write_register(IMU660RC_FUNC_CFG_ACCESS, 0x04); system_delay_ms(30); // 启用块更新功能与地址递增 imu660rc_write_register(IMU660RC_CTRL3, 0x44); switch(IMU660RC_ACC_SAMPLE_DEFAULT) { default: { zf_log(0, "IMU660RC_ACC_SAMPLE_DEFAULT set error."); return_state = 1; }break; case IMU660RC_ACC_SAMPLE_SGN_2G: { imu660rc_write_register(IMU660RC_CTRL8, 0x00); imu660rc_transition_factor[0] = 16393.44; }break; case IMU660RC_ACC_SAMPLE_SGN_4G: { imu660rc_write_register(IMU660RC_CTRL8, 0x01); imu660rc_transition_factor[0] = 8196.72; }break; case IMU660RC_ACC_SAMPLE_SGN_8G: { imu660rc_write_register(IMU660RC_CTRL8, 0x02); imu660rc_transition_factor[0] = 4098.36; }break; case IMU660RC_ACC_SAMPLE_SGN_16G: { imu660rc_write_register(IMU660RC_CTRL8, 0x03); imu660rc_transition_factor[0] = 2049.18; }break; } if(1 == return_state) { break; } switch(IMU660RC_GYRO_SAMPLE_DEFAULT) { default: { zf_log(0, "IMU660RC_GYRO_SAMPLE_DEFAULT set error."); return_state = 1; }break; case IMU660RC_GYRO_SAMPLE_SGN_125DPS: { imu660rc_write_register(IMU660RC_CTRL6, 0x00); imu660rc_transition_factor[1] = 228.5714; }break; case IMU660RC_GYRO_SAMPLE_SGN_250DPS: { imu660rc_write_register(IMU660RC_CTRL6, 0x01); imu660rc_transition_factor[1] = 114.2857; }break; case IMU660RC_GYRO_SAMPLE_SGN_500DPS: { imu660rc_write_register(IMU660RC_CTRL6, 0x02); imu660rc_transition_factor[1] = 57.1428; }break; case IMU660RC_GYRO_SAMPLE_SGN_1000DPS: { imu660rc_write_register(IMU660RC_CTRL6, 0x03); imu660rc_transition_factor[1] = 28.5714; }break; case IMU660RC_GYRO_SAMPLE_SGN_2000DPS: { imu660rc_write_register(IMU660RC_CTRL6, 0x04); imu660rc_transition_factor[1] = 14.2857; }break; case IMU660RC_GYRO_SAMPLE_SGN_4000DPS: { imu660rc_write_register(IMU660RC_CTRL6, 0x0C); imu660rc_transition_factor[1] = 7.14285; }break; } if(1 == return_state) { break; } // 设置传感器模式，设置为高精度模式以及输出速率 imu660rc_write_register(IMU660RC_CTRL1, 0x15); imu660rc_write_register(IMU660RC_CTRL2, 0x18); // 开启LPF1滤波器 imu660rc_write_register(IMU660RC_CTRL7, 0x01); // 开启LPF2滤波器 imu660rc_write_register(IMU660RC_CTRL9, 0x08); // 如果启用四元数输出，则配置相应数据 if(IMU660RC_QUARTERNION_DISABLE != quarternion_rate) { // 设置中断触发信号 imu660rc_write_register(IMU660RC_INT2_CTRL, 0x80); imu660rc_write_register(IMU660RC_CTRL4, 0x08); imu660rc_write_register(IMU660RC_EMB_FUNC_CFG, 0x30); // 重新配置加速度、角速度输出速率 imu660rc_write_register(IMU660RC_CTRL1, 0x10 | (quarternion_rate + 3)); imu660rc_write_register(IMU660RC_CTRL2, 0x10 | (quarternion_rate + 3)); // 设置四元数输出速率并开启 imu660rc_set_mem_bank(IMU660RC_EMBED_MEM_BANK); imu660rc_write_register(IMU660RC_SFLP_ODR, 0x43 | (quarternion_rate << 3)); imu660rc_write_register(IMU660RC_EMB_FUNC_EN_A, 0x02); imu660rc_write_register(IMU660RC_PAGE_RW, 0x00); imu660rc_set_mem_bank(IMU660RC_MAIN_MEM_BANK); // 开启中断检测引脚 exti_init(IMU660RC_INT2_PIN, EXTI_TRIGGER_RISING); } }while(0); return return_state; }