MPU6050姿态角DMP库解算，基于MSPM0G3507_mpu6050 dmp库
基于MSPM0G3507的学长自研学习板，学习了一下IIC和SPI
首先进行IIC读取MPU6050，硬件IIC的配置代码还有问题目前也还是不能使用的，我为了省时就采取软件模拟实现 接下来看具体的吧
IIC的基础知识我就不讲了，江协科技讲的很清楚了他的ppt在桌面stm32资料中， 讲一下学习的事情吧，要实现IIC读取mpu6050 我就先去网上找找现成的代码去了， 果然有有人已经做过，而且芯片一样（2024电赛原因，这看起来也还是挺正常的） 那篇博客已收藏CSDN但是代码大部分没有问题但在关键部分出问题了- 博客在这里
在这个地方还是卡了一下午，本以为博主的代码可以直接用的，但是在引脚部分出问题了导致没出结果找了一下，具体错误想不起来了，这部分的问题较少接着我就去搞板载外部Flash的读写了
因为spi的基础代码是好的我可以直接移植其他功能函数来使用，在移植这种代码时有时候有些函数不能很快找到可以借助AI帮你大概移植，又实在找不到的函数就老老实实goto道函数内层,然后可以直接操作底层或者找和他功能差不多的函数，在这部分任务中还是比较轻松的没遇到太大问题，但是对调试的过程还是记忆尤新，大概就是使用逻辑分析仪不断打波在上位机的设置上还是要说一下的，iic大概就是1MHZ的采样频率，而spi要比iic快很多，我用的是8MHZ的采样频率`，解析的数据很完整，还有逻辑分析上位机自带的协议解码器真好用，这里附上图片
然后又来搞IIC了，这次驱动温湿度计SHT401,相关资料都在test代码文件夹中，这部分就比较搞人了，查手册和官方代码 一步一步才实现了该功能
这部分的一个坑主要在于我要看懂他的代码，他的例程代码非常的有风格，代码在GitHub上，首先是初始化部分，在这时我还没有意识到芯片是需要唤醒，前面做陀螺仪时知道要进行唤醒，就是一系 列对陀螺仪本身寄存器的操作，但是并不在意这个东西，因为这些部分都是别人把代码做好了我直接套用的,这次虽然给了代码但是代码量稍微有点大，而且我看的一套代码是旧版的不是SHT401的是SHT3的，这两的操作和代码差异很大，不过功能部分还是通用的 SHT401_WriteReg(SHT4X_ADDRESS,SHT4X_CMD_READ_SERIAL);就是这一句就完成了初始化，在原3版中他有一堆的寄存器操作，这个就直接集成了，而发现这一问题我花了很长时间，首先我在看手册上的伪代码以为直接发送0xFD就可以得到数据了，里面完全不提具体的操作这个是我没想到的，最终还是通过在例程代码里翻才找到问题所在的，然后更多的时间花到了功能函数的移植过程中，它自带的函数做的是真有水平啊，一步一校验的代码，看的我人麻了不过多看看的话还好，直到在逻辑分析仪上能看到该有的数据时才歇了一口气，这时新问题又来了发送的指令是对了但是没有收到应答也就是跟没收到指令一样，读不到任何数据数据线一直拉高，这里又卡了我好久，我但是就非常的无语发了0xFD的数据读取指令结果在读地址的后面就不应答了，这个时候我就很想知道他的时序结果应该是个啥样的，因为这个时候芯片唤醒了，指令是对的，就是他没有对我的读指令进行应答，后面实在没有办法找不到问题了，有乖乖回去看手册去了，哈哈书中自有黄金屋，幸亏手册上加上了时序图还挺清晰明了的，这时才发现，那个应答位是我要发给温湿度计的不是等着芯片来回应，/** * 函 数：I2C发送应答位 * 参 数：Byte 要发送的应答位，范围：0~1，0表示应答，1表示非应答 * 返 回 值：无 */void MyI2C_SendAck(uint8_t AckBit){ SDA_OUT();MyI2C_W_SDA(AckBit);//主机把应答位数据放到SDA线MyI2C_W_SCL(1);//释放SCL，从机在SCL高电平期间，读取应答位MyI2C_W_SCL(0);//拉低SCL，开始下一个时序模块}自此这部分难点差不多了，这一部分就差不多完成了,下面是相关图片
最后这一部分也是最开始的一部分，也是最击溃我心灵的一部分，陀螺仪姿态角解算
事情是这样的，这个姿态角解算就是个变态，他给我六个加速度数据让我把它变成静态角度数据，这尼玛的看起来好像不难，你妹的差点没给我崩溃掉，因为我发现我最开始做出来陀螺的六轴加速度值如果不搞搞那种算法啥啥的好像就啥用没有，我对那种算法啥的不太确定没有啥信心，主要是有一个更有信心的方法就是驱动陀螺仪自带的DMP库，搞到这里才发现从一开始就可以直接移植正点原子的MPU6050就一切就都over了，也不一定开个玩笑，很多人在这个地方搞不下去了，甚至很多人的问题都一样，答案也是五花八门的，有用的答案又很少，我这边有几个卡我的问题我记得比较清楚的1.初始化部分
//mpu6050,dmp初始化//返回值:0,正常// 其他,失败u8 mpu_dmp_init(void){u8 res=0;int error = 0;error = mpu_init();if(error == 0)//初始化MPU6050{res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器if(res)return 1; res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFOif(res)return 2; res=mpu_set_sample_rate(DEFAULT_MPU_HZ);//设置采样率if(res)return 3; res=dmp_load_motion_driver_firmware();//加载dmp固件if(res)return 4;res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向if(res)return 5; res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|//设置dmp功能 DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO| DMP_FEATURE_GYRO_CAL);if(res)return 6; res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);//设置DMP输出速率(最大不超过200Hz)if(res)return 7; res=run_self_test();//自检if(res)return 8; res=mpu_set_dmp_state(1);//使能DMPif(res)return 9; }printf(\"MPU6050 init complete \\n\");return 0;}这部分卡在了第四步
res=dmp_load_motion_driver_firmware();//加载dmp固件if(res)return 4;往函数里面跳,这里可以借鉴的就是下面的printf大法，学会打印日志方便自己找错误，具体就是带上函数名再加上适当的标记
/** * @brief Load and verify DMP image. * @param[in] length Length of DMP image. * @param[in] firmware DMP code. * @param[in] start_addr Starting address of DMP code memory. * @param[in] sample_rate Fixed sampling rate used when DMP is enabled. * @return 0 if successful. */int mpu_load_firmware(unsigned short length, const unsigned char *firmware, unsigned short start_addr, unsigned short sample_rate){ unsigned short ii; unsigned short this_write; /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */#define LOAD_CHUNK (16) unsigned char cur[LOAD_CHUNK], tmp[2]; if (st.chip_cfg.dmp_loaded){printf(\"dmp_load_motion_driver_firmware() mpu_load_firmware() error return 1\\n\"); /* DMP should only be loaded once. */ return -1;} if (!firmware){printf(\"dmp_load_motion_driver_firmware() mpu_load_firmware() error return 2\\n\"); return -1;} for (ii = 0; ii < length; ii += this_write) { this_write = min(LOAD_CHUNK, length - ii); if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii])){printf(\"dmp_load_motion_driver_firmware() mpu_load_firmware() error return 3\\n\"); return -1;} if (mpu_read_mem(ii, this_write, cur)){printf(\"dmp_load_motion_driver_firmware() mpu_load_firmware() error return 4\\n\"); return -1;} if (memcmp(firmware+ii, cur, this_write)){printf(\"dmp_load_motion_driver_firmware() mpu_load_firmware() error return 5 ii = %d \\n\",ii); /* DMP should only be loaded once. */ return -2;} } /* Set program start address. */ tmp[0] = start_addr >> 8; tmp[1] = start_addr & 0xFF; if (i2c_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp)) return -1; st.chip_cfg.dmp_loaded = 1; st.chip_cfg.dmp_sample_rate = sample_rate;printf(\"mpu_load_firmware() OK return 0\\n\"); return 0;}下面是报错的地方，问题在于给MPU6050内置的DMP加载固件(下代码)时写入缓冲区的数据，再被读取时有出入，从逻辑分析仪上可以看到写入数据是对的而且成功被收到
if (memcmp(firmware+ii, cur, this_write)){printf(\"dmp_load_motion_driver_firmware() mpu_load_firmware() error return 5 ii = %d \\n\",ii); /* DMP should only be loaded once. */ return -2;}所以问题就在于读取部分,上面这段代码在网上有很多人都遇到了相同的问题，这个地方是在CSDN逛出来的，找了一篇就是讲这个问题的，但是博文很不起眼，我就抱着试一试的心态试了一下，确实不在这报错了
这是博客
但是问题还没有结束，初始化是过去了但是数据不对，mpu6050dmp读数据为nan然后又开始追根溯源
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)//pitch:俯仰角 精度:0.1° 范围:-90.0° +90.0°//roll:横滚角 精度:0.1° 范围:-180.0° +180.0°//yaw:航向角 精度:0.1° 范围:-180.0° +180.0°//返回值:0,正常// 其他,失败u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw){float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;unsigned long sensor_timestamp;short gyro[3], accel[3], sensors;unsigned char more;long quat[4]; if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1; /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units. * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.**//*if (sensors & INV_XYZ_GYRO )send_packet(PACKET_TYPE_GYRO, gyro);if (sensors & INV_XYZ_ACCEL)send_packet(PACKET_TYPE_ACCEL, accel); *//* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30. * The orientation is set by the scalar passed to dmp_set_orientation during initialization. **/printf(\"\\nquat[0] = %ld quat[1] = %ld quat[2] = %ld quat[3] = %ld\\n \",quat[0],quat[1],quat[2],quat[3]);if(sensors&INV_WXYZ_QUAT){q0 = quat[0] / q30;//q30格式转换为浮点数q1 = quat[1] / q30;q2 = quat[2] / q30;q3 = quat[3] / q30;//计算得到俯仰角/横滚角/航向角*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;// pitch*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;// roll*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;//yaw}else return 2;return 0;}这里我直接打印四元数，结果都有数据就是大了点，然后我将公式拿到vc中进行运算，竟然可以得到数据，这就很让人无语了，后来还是在CSDN逛到了一篇博客
这就是最让我感激的一个博客，叫我自己想怕是想不到这个问题的，就没有办法了试了一下，哈哈结果就出来了，在这一刻后我才放松了下来
