Initial commit: TC264 smart car project with IPS200 menu system
- 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
This commit is contained in:
32343 committed
commit
843447835d
660 files changed
+443632
No files matched your search
@@ -0,0 +1,563 @@
|
||||
/*********************************************************************************************************************
|
||||
* 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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in new issue
Block a user