JY61P姿态传感器I2C驱动移植实战基于TI MSPM0G3507开发板的卡尔曼滤波数据读取与解析最近在做一个基于TI MSPM0G3507的平衡小车项目需要用到姿态传感器来检测车体的倾斜角度。选来选去最终用了维特智能的JY61P这款六轴姿态传感器。它内部集成了三轴陀螺仪和三轴加速度计最关键的是它自带了基于卡尔曼滤波的融合算法直接输出处理好的横滚、俯仰、航向角省去了我们自己写复杂滤波算法的麻烦。不过在把它接到MSPM0G3507开发板上时还是遇到了一些小坑。官方资料主要是针对串口通信的而我想用I2C接口来节省引脚。折腾了一番后终于把驱动成功移植了过来。今天我就把整个移植过程、代码编写以及数据解析的细节手把手地分享给大家希望能帮到正在做类似项目的朋友。1. 认识JY61P你的三维姿态“感知官”在开始写代码之前咱们先花几分钟了解一下JY61P到底是个啥以及它能干什么。JY61P是一个基于MEMS微机电系统技术的高性能三维运动姿态测量模块。简单来说它内部有六个“感觉器官”三轴陀螺仪感受物体绕X、Y、Z三个轴旋转的角速度转得多快。三轴加速度计感受物体在X、Y、Z三个方向上的线性加速度包括重力加速度。如果只有这些原始数据我们还得费很大劲去计算姿态角。JY61P厉害的地方在于它内部有一颗处理器运行着维特智能自主研发的姿态动力学核心算法引擎和高动态卡尔曼滤波融合算法。提示卡尔曼滤波你可以理解为一个非常聪明的“数据融合大师”。它能把陀螺仪短期精度高但会漂移和加速度计长期稳定但动态响应慢的数据结合起来取长补短最终输出既稳定又响应快的姿态角度。最终这个模块通过串口或I2C接口直接给我们提供已经计算好的、实时补偿后的横滚角(Roll)、俯仰角(Pitch)、航向角(Yaw)数据更新率最高能达到200Hz完全能满足大多数高精度动作捕捉和姿态估计的需求。它的基本规格如下工作电压5V通信接口串口默认或 I2C我们本次使用数据回传速率最高200Hz引脚12 Pin (2.54mm间距)2. 准备工作硬件连接与资料获取动手写代码前先把硬件连好并把必要的资料准备好。2.1 硬件连接我们需要用到开发板的I2C接口和电源。根据原文的指引连接非常简单JY61P模块引脚MSPM0G3507开发板引脚说明VCC5V05V电源GNDGND地线SCLPA1I2C时钟线SDAPA0I2C数据线注意务必确认开发板上的5V0引脚有输出并且能提供足够的电流。连接时最好先断电操作。2.2 关键资料获取在开发过程中下面这几个文档和工具会非常有用建议提前下载好产品规格书与资料里面包含了传感器的详细参数、电气特性、通信协议等。这是我们的“字典”。链接JY61P产品资料通信协议文档这是本次移植的核心里面定义了所有寄存器的地址、功能以及读写时序。链接https://wit-motion.yuque.com/wimwnr/ltst03/vl3tpy?#UFGm8在这个页面你可以找到每个寄存器的ADDR(HEX)地址、FUNCTION功能和SERIAL I/F读写权限R/W代表可读写R代表只读。上位机软件用于初始测试和配置传感器比如检查模块是否正常工作或进行初始的零位校准。下载地址https://wit-motion.yuque.com/wimwnr/aqvq6y/qngktvx5grz81zkq3. 核心实战I2C驱动移植与代码编写好了硬件连好了资料也齐了现在进入最关键的代码部分。我会按照实际开发的顺序一步步带你完成。3.1 工程配置与引脚定义首先我们需要在CCS工程中配置用于模拟I2C的GPIO引脚。原文中使用的是SysConfig图形化配置工具非常直观。在工程中找到并双击empty.syscfg文件。在界面中添加两个GPIO组件分别对应PA0 (SDA) 和 PA1 (SCL)。将它们都配置为通用输出因为我们要用软件模拟I2C时序需要控制输出高低电平同时为SDA引脚也开启输入功能因为读取数据时需要切换为输入模式。按Ctrl S保存配置。点击编译工程。这时系统会自动生成ti_msp_dl_config.h等配置文件里面包含了我们刚定义的引脚宏如IIC_Software_SDA_PIN。我们的工程已经包含了board.h而board.h又包含了ti_msp_dl_config.h所以后续我们只需要在代码里#include board.h就能使用这些引脚定义了。3.2 建立驱动文件在项目的BSP板级支持包文件夹下新建inc和src子文件夹。然后创建两个文件src/bsp_gyro.c驱动源文件存放所有函数实现。inc/bsp_gyro.h驱动头文件存放宏定义、函数声明和结构体。别忘了在CCS的工程属性中将inc文件夹的路径添加到编译器的包含路径Include Path里。3.3 头文件bsp_gyro.h详解头文件里定义了我们驱动需要用到的所有“零件”。咱们先来看看#ifndef __BSP_GYRO_H__ #define __BSP_GYRO_H__ #include board.h // 调试开关设置为1时会通过串口打印原始字节数据方便调试 #define GYRO_DEBUG 0 // 定义一个结构体来存储最终计算出的角度值 typedef struct { float x; // 横滚角 Roll float y; // 俯仰角 Pitch float z; // 航向角 Yaw } Gyro_Struct; // ---------- 关键寄存器地址定义 ---------- #define IIC_ADDR 0x50 // JY61P的I2C设备地址 #define UN_REG 0x69 // 寄存器解锁命令的寄存器地址 #define SAVE_REG 0x00 // 保存寄存器地址 #define ANGLE_REFER_REG 0x01 // 角度参考归零寄存器地址 // 横滚角起始寄存器是0x3D我们在读函数里直接用了 // ---------- 软件I2C引脚操作宏定义 ---------- // 将SDA引脚设置为输入模式读取数据时用 #define SDA_IN() { DL_GPIO_initDigitalInput(IIC_Software_SDA_IOMUX); } // 将SDA引脚设置为输出模式发送数据时用 #define SDA_OUT() { DL_GPIO_initDigitalOutput(IIC_Software_SDA_IOMUX); \ DL_GPIO_enableOutput(IIC_Software_PORT, IIC_Software_SDA_PIN); \ } // 控制SCL引脚输出高或低 #define SCL(BIT) ( BIT ? DL_GPIO_setPins(IIC_Software_PORT,IIC_Software_SCL_PIN) : DL_GPIO_clearPins(IIC_Software_PORT,IIC_Software_SCL_PIN) ) // 控制SDA引脚输出高或低输出模式时 #define SDA(BIT) ( BIT ? DL_GPIO_setPins(IIC_Software_PORT,IIC_Software_SDA_PIN) : DL_GPIO_clearPins(IIC_Software_PORT,IIC_Software_SDA_PIN) ) // 读取SDA引脚的电平状态输入模式时 #define SDA_GET() ( ( DL_GPIO_readPins( IIC_Software_PORT, IIC_Software_SDA_PIN ) IIC_Software_SDA_PIN ) ? 1 : 0 ) // 函数声明 void jy61pInit(void); // 传感器初始化包括归零操作 uint8_t readDataJy61p(uint8_t dev, uint8_t reg, uint8_t *data, uint32_t length); // I2C读函数 uint8_t writeDataJy61p(uint8_t dev, uint8_t reg, uint8_t* data, uint32_t length); // I2C写函数 Gyro_Struct *get_angle(void); // 获取角度数据的主函数 #endif3.4 核心函数实现bsp_gyro.c这里包含了I2C底层时序、读写封装以及角度获取的所有逻辑。代码有点长我们拆解开来重点看几个关键部分。3.4.1 传感器初始化与归零 (jy61pInit)在开始读取数据前我们通常希望以传感器当前的姿态作为“零位”。JY61P提供了寄存器归零功能。特别注意修改任何配置寄存器都需要遵循“解锁-设置-保存”的流程。void jy61pInit(void) { /* Z轴归零 */ // 1. 寄存器解锁向0x69寄存器写入0xB588 uint8_t unlock_reg1[2] {0x88, 0xB5}; // 注意先发低位0x88再发高位0xB5 writeDataJy61p(IIC_ADDR, UN_REG, unlock_reg1, 2); delay_ms(200); // 等待传感器处理 // 2. Z轴归零向0x01寄存器写入0x0004 uint8_t z_axis_reg[2] {0x04, 0x00}; writeDataJy61p(IIC_ADDR, ANGLE_REFER_REG, z_axis_reg, 2); delay_ms(200); // 3. 保存配置向0x00寄存器写入0x0000 uint8_t save_reg1[2] {0x00, 0x00}; writeDataJy61p(IIC_ADDR, SAVE_REG, save_reg1, 2); delay_ms(200); /* 角度归零可选*/ // 如果你希望所有角度Roll, Pitch, Yaw都清零可以重复上述流程但第二步数据改为0x0008 // 寄存器解锁 uint8_t unlock_reg[2] {0x88, 0xB5}; writeDataJy61p(IIC_ADDR, UN_REG, unlock_reg, 2); delay_ms(200); // 角度归零 uint8_t angle_reg[2] {0x08, 0x00}; // 注意这里是0x0008 writeDataJy61p(IIC_ADDR, ANGLE_REFER_REG, angle_reg, 2); delay_ms(200); // 保存 uint8_t save_reg[2] {0x00, 0x00}; writeDataJy61p(IIC_ADDR, SAVE_REG, save_reg, 2); delay_ms(200); // 初始化用于存储角度的结构体 memset((void *)Gyro_Structure, 0, sizeof(Gyro_Structure)); }注意原文中提到资料要求延时3秒但实测200ms也可行。在实际项目中为了可靠性我建议至少延时500ms-1s避免因传感器处理不及时导致配置失败。3.4.2 I2C读写函数 (writeDataJy61preadDataJy61p)这两个函数是对底层I2C时序的封装是驱动传感器的基石。writeDataJy61p用于向指定寄存器写入数据readDataJy61p用于从指定寄存器读取数据。这里重点看一下readDataJy61p函数因为它有一个关键点uint8_t readDataJy61p(uint8_t dev, uint8_t reg, uint8_t *data, uint32_t length) { uint32_t count 0; IIC_Start(); // 发送设备地址写模式以指定要读的寄存器 Send_Byte((dev1)|0); if(I2C_WaitAck() 1) return 0; // 发送寄存器地址 Send_Byte(reg); if(I2C_WaitAck() 1) return 0; delay_us(5); // 稍作延时 // 重新发送起始信号并切换为读模式 IIC_Start(); Send_Byte((dev1)|1); if(I2C_WaitAck() 1) return 0; // 循环读取数据 for(count0; countlength; count) { if(count ! length-1) { // 如果不是最后一个字节发送ACK低电平告诉传感器“请继续发送下一个字节” data[count] Read_Byte(); IIC_Send_Ack(0); } else { // 如果是最后一个字节发送NACK高电平告诉传感器“停止发送” data[count] Read_Byte(); IIC_Send_Ack(1); } } IIC_Stop(); return 1; }关键理解I2C协议中主机每接收一个字节后都需要向从机发送一个应答信号(ACK)。发送ACK(0)表示“我收到了请发下一个”发送NACK(1)表示“我收到了但不用再发了”。这个逻辑在for循环里的if-else语句中实现必须严格遵循否则数据读取会出错。3.4.3 获取并解析角度数据 (get_angle)这是最上层的应用函数我们调用它就能直接拿到处理好的角度值。Gyro_Struct *get_angle(void) { uint8_t sda_angle[6] {0}; // 缓存从传感器读回的6个原始字节 int ret 0; memset((void *)sda_angle, 0, sizeof(sda_angle)); // 从0x3D寄存器开始连续读取6个字节 // 这6个字节依次是Roll低、Roll高、Pitch低、Pitch高、Yaw低、Yaw高 ret readDataJy61p(IIC_ADDR, 0x3D, sda_angle, 6); if(ret 0) { lc_printf(\nRead Error\r\n); // 读取失败处理 return Gyro_Structure; } // 调试信息打印原始字节 #if GYRO_DEBUG lc_printf(RollL %x\r\n, sda_angle[0]); lc_printf(RollH %x\r\n, sda_angle[1]); // ... 打印其他字节 #endif // ---------- 核心数据解析 ---------- // 1. 将两个字节组合成一个16位有符号整数 // 2. 根据手册传感器输出的原始值范围是-32768 ~ 32767对应 -180° ~ 180° // 3. 转换为浮点数角度 // 解析横滚角 Roll (X轴) int16_t roll_raw (sda_angle[1] 8) | sda_angle[0]; float RollX (float)roll_raw / 32768.0 * 180.0; // 解析俯仰角 Pitch (Y轴) int16_t pitch_raw (sda_angle[3] 8) | sda_angle[2]; float PitchY (float)pitch_raw / 32768.0 * 180.0; // 解析航向角 Yaw (Z轴) int16_t yaw_raw (sda_angle[5] 8) | sda_angle[4]; float YawZ (float)yaw_raw / 32768.0 * 180.0; // 将角度值规整到 -180° 到 180° 的范围内虽然计算本身应该就在这个范围但做个保护 if (RollX 180.0) RollX - 360.0; else if (RollX -180.0) RollX 360.0; // PitchY 和 YawZ 做同样处理代码已省略原理相同 // 存入结构体并返回 Gyro_Structure.x RollX; Gyro_Structure.y PitchY; Gyro_Structure.z YawZ; return Gyro_Structure; }4. 在主程序中调用与验证驱动写好了最后就是在主函数里调用它了。代码非常简洁#include ti_msp_dl_config.h #include board.h #include bsp_gyro.h int main(void) { SYSCFG_DL_init(); // 系统初始化 jy61pInit(); // 初始化JY61P传感器并进行归零操作 lc_printf(\r\nJY61P 3D Attitude Measurement Sensor Start...\r\n); while (1) { // 获取角度数据 Gyro_Struct *JY61P_Data get_angle(); // 通过串口打印角度值这里转换为整数显示可根据需要改为浮点 lc_printf(JY61P RollX [ %d ]\r\n, (int)JY61P_Data-x); lc_printf(JY61P PitchY [ %d ]\r\n, (int)JY61P_Data-y); lc_printf(JY61P YawZ [ %d ]\r\n, (int)JY61P_Data-z); lc_printf(\n); delay_ms(100); // 每100ms读取一次即10Hz } }将程序编译下载到MSPM0G3507开发板打开串口助手波特率与你的lc_printf设置一致你应该能看到类似下面的输出JY61P 3D Attitude Measurement Sensor Start... JY61P RollX [ 0 ] JY61P PitchY [ 0 ] JY61P YawZ [ 45 ] JY61P RollX [ 1 ] JY61P PitchY [ -1 ] JY61P YawZ [ 45 ]当你旋转开发板连着传感器时Roll、Pitch、Yaw的数值会随之变化。恭喜你你已经成功在TI MSPM0G3507上驱动了JY61P姿态传感器几个调试小贴士如果读不到数据首先检查硬件连接电源、I2C线是否接反然后用逻辑分析仪或示波器抓一下PA0和PA1的波形看I2C时序是否正确特别是起始、停止和ACK信号。如果数据不对打开GYRO_DEBUG宏查看从传感器读回来的6个原始字节是否正确。再对照数据解析公式检查计算过程。关于航向角(Yaw)模块上电后航向角是以初始位置为0点计算的。由于没有磁力计纯靠陀螺积分Yaw角在长时间静止后可能会有缓慢漂移这是正常现象。对于需要绝对航向的应用建议选择带磁力计的九轴模块。
JY61P姿态传感器I2C驱动移植实战：基于TI MSPM0G3507开发板的卡尔曼滤波数据读取与解析
JY61P姿态传感器I2C驱动移植实战基于TI MSPM0G3507开发板的卡尔曼滤波数据读取与解析最近在做一个基于TI MSPM0G3507的平衡小车项目需要用到姿态传感器来检测车体的倾斜角度。选来选去最终用了维特智能的JY61P这款六轴姿态传感器。它内部集成了三轴陀螺仪和三轴加速度计最关键的是它自带了基于卡尔曼滤波的融合算法直接输出处理好的横滚、俯仰、航向角省去了我们自己写复杂滤波算法的麻烦。不过在把它接到MSPM0G3507开发板上时还是遇到了一些小坑。官方资料主要是针对串口通信的而我想用I2C接口来节省引脚。折腾了一番后终于把驱动成功移植了过来。今天我就把整个移植过程、代码编写以及数据解析的细节手把手地分享给大家希望能帮到正在做类似项目的朋友。1. 认识JY61P你的三维姿态“感知官”在开始写代码之前咱们先花几分钟了解一下JY61P到底是个啥以及它能干什么。JY61P是一个基于MEMS微机电系统技术的高性能三维运动姿态测量模块。简单来说它内部有六个“感觉器官”三轴陀螺仪感受物体绕X、Y、Z三个轴旋转的角速度转得多快。三轴加速度计感受物体在X、Y、Z三个方向上的线性加速度包括重力加速度。如果只有这些原始数据我们还得费很大劲去计算姿态角。JY61P厉害的地方在于它内部有一颗处理器运行着维特智能自主研发的姿态动力学核心算法引擎和高动态卡尔曼滤波融合算法。提示卡尔曼滤波你可以理解为一个非常聪明的“数据融合大师”。它能把陀螺仪短期精度高但会漂移和加速度计长期稳定但动态响应慢的数据结合起来取长补短最终输出既稳定又响应快的姿态角度。最终这个模块通过串口或I2C接口直接给我们提供已经计算好的、实时补偿后的横滚角(Roll)、俯仰角(Pitch)、航向角(Yaw)数据更新率最高能达到200Hz完全能满足大多数高精度动作捕捉和姿态估计的需求。它的基本规格如下工作电压5V通信接口串口默认或 I2C我们本次使用数据回传速率最高200Hz引脚12 Pin (2.54mm间距)2. 准备工作硬件连接与资料获取动手写代码前先把硬件连好并把必要的资料准备好。2.1 硬件连接我们需要用到开发板的I2C接口和电源。根据原文的指引连接非常简单JY61P模块引脚MSPM0G3507开发板引脚说明VCC5V05V电源GNDGND地线SCLPA1I2C时钟线SDAPA0I2C数据线注意务必确认开发板上的5V0引脚有输出并且能提供足够的电流。连接时最好先断电操作。2.2 关键资料获取在开发过程中下面这几个文档和工具会非常有用建议提前下载好产品规格书与资料里面包含了传感器的详细参数、电气特性、通信协议等。这是我们的“字典”。链接JY61P产品资料通信协议文档这是本次移植的核心里面定义了所有寄存器的地址、功能以及读写时序。链接https://wit-motion.yuque.com/wimwnr/ltst03/vl3tpy?#UFGm8在这个页面你可以找到每个寄存器的ADDR(HEX)地址、FUNCTION功能和SERIAL I/F读写权限R/W代表可读写R代表只读。上位机软件用于初始测试和配置传感器比如检查模块是否正常工作或进行初始的零位校准。下载地址https://wit-motion.yuque.com/wimwnr/aqvq6y/qngktvx5grz81zkq3. 核心实战I2C驱动移植与代码编写好了硬件连好了资料也齐了现在进入最关键的代码部分。我会按照实际开发的顺序一步步带你完成。3.1 工程配置与引脚定义首先我们需要在CCS工程中配置用于模拟I2C的GPIO引脚。原文中使用的是SysConfig图形化配置工具非常直观。在工程中找到并双击empty.syscfg文件。在界面中添加两个GPIO组件分别对应PA0 (SDA) 和 PA1 (SCL)。将它们都配置为通用输出因为我们要用软件模拟I2C时序需要控制输出高低电平同时为SDA引脚也开启输入功能因为读取数据时需要切换为输入模式。按Ctrl S保存配置。点击编译工程。这时系统会自动生成ti_msp_dl_config.h等配置文件里面包含了我们刚定义的引脚宏如IIC_Software_SDA_PIN。我们的工程已经包含了board.h而board.h又包含了ti_msp_dl_config.h所以后续我们只需要在代码里#include board.h就能使用这些引脚定义了。3.2 建立驱动文件在项目的BSP板级支持包文件夹下新建inc和src子文件夹。然后创建两个文件src/bsp_gyro.c驱动源文件存放所有函数实现。inc/bsp_gyro.h驱动头文件存放宏定义、函数声明和结构体。别忘了在CCS的工程属性中将inc文件夹的路径添加到编译器的包含路径Include Path里。3.3 头文件bsp_gyro.h详解头文件里定义了我们驱动需要用到的所有“零件”。咱们先来看看#ifndef __BSP_GYRO_H__ #define __BSP_GYRO_H__ #include board.h // 调试开关设置为1时会通过串口打印原始字节数据方便调试 #define GYRO_DEBUG 0 // 定义一个结构体来存储最终计算出的角度值 typedef struct { float x; // 横滚角 Roll float y; // 俯仰角 Pitch float z; // 航向角 Yaw } Gyro_Struct; // ---------- 关键寄存器地址定义 ---------- #define IIC_ADDR 0x50 // JY61P的I2C设备地址 #define UN_REG 0x69 // 寄存器解锁命令的寄存器地址 #define SAVE_REG 0x00 // 保存寄存器地址 #define ANGLE_REFER_REG 0x01 // 角度参考归零寄存器地址 // 横滚角起始寄存器是0x3D我们在读函数里直接用了 // ---------- 软件I2C引脚操作宏定义 ---------- // 将SDA引脚设置为输入模式读取数据时用 #define SDA_IN() { DL_GPIO_initDigitalInput(IIC_Software_SDA_IOMUX); } // 将SDA引脚设置为输出模式发送数据时用 #define SDA_OUT() { DL_GPIO_initDigitalOutput(IIC_Software_SDA_IOMUX); \ DL_GPIO_enableOutput(IIC_Software_PORT, IIC_Software_SDA_PIN); \ } // 控制SCL引脚输出高或低 #define SCL(BIT) ( BIT ? DL_GPIO_setPins(IIC_Software_PORT,IIC_Software_SCL_PIN) : DL_GPIO_clearPins(IIC_Software_PORT,IIC_Software_SCL_PIN) ) // 控制SDA引脚输出高或低输出模式时 #define SDA(BIT) ( BIT ? DL_GPIO_setPins(IIC_Software_PORT,IIC_Software_SDA_PIN) : DL_GPIO_clearPins(IIC_Software_PORT,IIC_Software_SDA_PIN) ) // 读取SDA引脚的电平状态输入模式时 #define SDA_GET() ( ( DL_GPIO_readPins( IIC_Software_PORT, IIC_Software_SDA_PIN ) IIC_Software_SDA_PIN ) ? 1 : 0 ) // 函数声明 void jy61pInit(void); // 传感器初始化包括归零操作 uint8_t readDataJy61p(uint8_t dev, uint8_t reg, uint8_t *data, uint32_t length); // I2C读函数 uint8_t writeDataJy61p(uint8_t dev, uint8_t reg, uint8_t* data, uint32_t length); // I2C写函数 Gyro_Struct *get_angle(void); // 获取角度数据的主函数 #endif3.4 核心函数实现bsp_gyro.c这里包含了I2C底层时序、读写封装以及角度获取的所有逻辑。代码有点长我们拆解开来重点看几个关键部分。3.4.1 传感器初始化与归零 (jy61pInit)在开始读取数据前我们通常希望以传感器当前的姿态作为“零位”。JY61P提供了寄存器归零功能。特别注意修改任何配置寄存器都需要遵循“解锁-设置-保存”的流程。void jy61pInit(void) { /* Z轴归零 */ // 1. 寄存器解锁向0x69寄存器写入0xB588 uint8_t unlock_reg1[2] {0x88, 0xB5}; // 注意先发低位0x88再发高位0xB5 writeDataJy61p(IIC_ADDR, UN_REG, unlock_reg1, 2); delay_ms(200); // 等待传感器处理 // 2. Z轴归零向0x01寄存器写入0x0004 uint8_t z_axis_reg[2] {0x04, 0x00}; writeDataJy61p(IIC_ADDR, ANGLE_REFER_REG, z_axis_reg, 2); delay_ms(200); // 3. 保存配置向0x00寄存器写入0x0000 uint8_t save_reg1[2] {0x00, 0x00}; writeDataJy61p(IIC_ADDR, SAVE_REG, save_reg1, 2); delay_ms(200); /* 角度归零可选*/ // 如果你希望所有角度Roll, Pitch, Yaw都清零可以重复上述流程但第二步数据改为0x0008 // 寄存器解锁 uint8_t unlock_reg[2] {0x88, 0xB5}; writeDataJy61p(IIC_ADDR, UN_REG, unlock_reg, 2); delay_ms(200); // 角度归零 uint8_t angle_reg[2] {0x08, 0x00}; // 注意这里是0x0008 writeDataJy61p(IIC_ADDR, ANGLE_REFER_REG, angle_reg, 2); delay_ms(200); // 保存 uint8_t save_reg[2] {0x00, 0x00}; writeDataJy61p(IIC_ADDR, SAVE_REG, save_reg, 2); delay_ms(200); // 初始化用于存储角度的结构体 memset((void *)Gyro_Structure, 0, sizeof(Gyro_Structure)); }注意原文中提到资料要求延时3秒但实测200ms也可行。在实际项目中为了可靠性我建议至少延时500ms-1s避免因传感器处理不及时导致配置失败。3.4.2 I2C读写函数 (writeDataJy61preadDataJy61p)这两个函数是对底层I2C时序的封装是驱动传感器的基石。writeDataJy61p用于向指定寄存器写入数据readDataJy61p用于从指定寄存器读取数据。这里重点看一下readDataJy61p函数因为它有一个关键点uint8_t readDataJy61p(uint8_t dev, uint8_t reg, uint8_t *data, uint32_t length) { uint32_t count 0; IIC_Start(); // 发送设备地址写模式以指定要读的寄存器 Send_Byte((dev1)|0); if(I2C_WaitAck() 1) return 0; // 发送寄存器地址 Send_Byte(reg); if(I2C_WaitAck() 1) return 0; delay_us(5); // 稍作延时 // 重新发送起始信号并切换为读模式 IIC_Start(); Send_Byte((dev1)|1); if(I2C_WaitAck() 1) return 0; // 循环读取数据 for(count0; countlength; count) { if(count ! length-1) { // 如果不是最后一个字节发送ACK低电平告诉传感器“请继续发送下一个字节” data[count] Read_Byte(); IIC_Send_Ack(0); } else { // 如果是最后一个字节发送NACK高电平告诉传感器“停止发送” data[count] Read_Byte(); IIC_Send_Ack(1); } } IIC_Stop(); return 1; }关键理解I2C协议中主机每接收一个字节后都需要向从机发送一个应答信号(ACK)。发送ACK(0)表示“我收到了请发下一个”发送NACK(1)表示“我收到了但不用再发了”。这个逻辑在for循环里的if-else语句中实现必须严格遵循否则数据读取会出错。3.4.3 获取并解析角度数据 (get_angle)这是最上层的应用函数我们调用它就能直接拿到处理好的角度值。Gyro_Struct *get_angle(void) { uint8_t sda_angle[6] {0}; // 缓存从传感器读回的6个原始字节 int ret 0; memset((void *)sda_angle, 0, sizeof(sda_angle)); // 从0x3D寄存器开始连续读取6个字节 // 这6个字节依次是Roll低、Roll高、Pitch低、Pitch高、Yaw低、Yaw高 ret readDataJy61p(IIC_ADDR, 0x3D, sda_angle, 6); if(ret 0) { lc_printf(\nRead Error\r\n); // 读取失败处理 return Gyro_Structure; } // 调试信息打印原始字节 #if GYRO_DEBUG lc_printf(RollL %x\r\n, sda_angle[0]); lc_printf(RollH %x\r\n, sda_angle[1]); // ... 打印其他字节 #endif // ---------- 核心数据解析 ---------- // 1. 将两个字节组合成一个16位有符号整数 // 2. 根据手册传感器输出的原始值范围是-32768 ~ 32767对应 -180° ~ 180° // 3. 转换为浮点数角度 // 解析横滚角 Roll (X轴) int16_t roll_raw (sda_angle[1] 8) | sda_angle[0]; float RollX (float)roll_raw / 32768.0 * 180.0; // 解析俯仰角 Pitch (Y轴) int16_t pitch_raw (sda_angle[3] 8) | sda_angle[2]; float PitchY (float)pitch_raw / 32768.0 * 180.0; // 解析航向角 Yaw (Z轴) int16_t yaw_raw (sda_angle[5] 8) | sda_angle[4]; float YawZ (float)yaw_raw / 32768.0 * 180.0; // 将角度值规整到 -180° 到 180° 的范围内虽然计算本身应该就在这个范围但做个保护 if (RollX 180.0) RollX - 360.0; else if (RollX -180.0) RollX 360.0; // PitchY 和 YawZ 做同样处理代码已省略原理相同 // 存入结构体并返回 Gyro_Structure.x RollX; Gyro_Structure.y PitchY; Gyro_Structure.z YawZ; return Gyro_Structure; }4. 在主程序中调用与验证驱动写好了最后就是在主函数里调用它了。代码非常简洁#include ti_msp_dl_config.h #include board.h #include bsp_gyro.h int main(void) { SYSCFG_DL_init(); // 系统初始化 jy61pInit(); // 初始化JY61P传感器并进行归零操作 lc_printf(\r\nJY61P 3D Attitude Measurement Sensor Start...\r\n); while (1) { // 获取角度数据 Gyro_Struct *JY61P_Data get_angle(); // 通过串口打印角度值这里转换为整数显示可根据需要改为浮点 lc_printf(JY61P RollX [ %d ]\r\n, (int)JY61P_Data-x); lc_printf(JY61P PitchY [ %d ]\r\n, (int)JY61P_Data-y); lc_printf(JY61P YawZ [ %d ]\r\n, (int)JY61P_Data-z); lc_printf(\n); delay_ms(100); // 每100ms读取一次即10Hz } }将程序编译下载到MSPM0G3507开发板打开串口助手波特率与你的lc_printf设置一致你应该能看到类似下面的输出JY61P 3D Attitude Measurement Sensor Start... JY61P RollX [ 0 ] JY61P PitchY [ 0 ] JY61P YawZ [ 45 ] JY61P RollX [ 1 ] JY61P PitchY [ -1 ] JY61P YawZ [ 45 ]当你旋转开发板连着传感器时Roll、Pitch、Yaw的数值会随之变化。恭喜你你已经成功在TI MSPM0G3507上驱动了JY61P姿态传感器几个调试小贴士如果读不到数据首先检查硬件连接电源、I2C线是否接反然后用逻辑分析仪或示波器抓一下PA0和PA1的波形看I2C时序是否正确特别是起始、停止和ACK信号。如果数据不对打开GYRO_DEBUG宏查看从传感器读回来的6个原始字节是否正确。再对照数据解析公式检查计算过程。关于航向角(Yaw)模块上电后航向角是以初始位置为0点计算的。由于没有磁力计纯靠陀螺积分Yaw角在长时间静止后可能会有缓慢漂移这是正常现象。对于需要绝对航向的应用建议选择带磁力计的九轴模块。
