32343
843447835d
- IPS200 display menu with real-time status and parameter settings - Camera image processing and line tracking - IMU attitude solution and PID control - WiFi SPI oscilloscope data transmission
564 lines
22 KiB
C
564 lines
22 KiB
C
/*********************************************************************************************************************
|
||
* RT1064DVL6A Opensourec Library 即（RT1064DVL6A 开源库）是一个基于官方 SDK 接口的第三方开源库
|
||
* Copyright (c) 2022 SEEKFREE 逐飞科技
|
||
*
|
||
* 本文件是 RT1064DVL6A 开源库的一部分
|
||
*
|
||
* RT1064DVL6A 开源库 是免费软件
|
||
* 您可以根据自由软件基金会发布的 GPL（GNU General Public License，即 GNU通用公共许可证）的条款
|
||
* 即 GPL 的第3版（即 GPL3.0）或（您选择的）任何后来的版本，重新发布和/或修改它
|
||
*
|
||
* 本开源库的发布是希望它能发挥作用，但并未对其作任何的保证
|
||
* 甚至没有隐含的适销性或适合特定用途的保证
|
||
* 更多细节请参见 GPL
|
||
*
|
||
* 您应该在收到本开源库的同时收到一份 GPL 的副本
|
||
* 如果没有，请参阅<https://www.gnu.org/licenses/>
|
||
*
|
||
* 额外注明：
|
||
* 本开源库使用 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;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|