MPU6050移植DMP读出三轴角度

news2024/12/30 2:33:36

先决条件

  • MPU6050移植完成，能够正确读出加速度和陀螺仪原始数据

DMP移植

官方包代码复制
  • 解压demo，打开motion_driver_6.12\arm\STM32F4_MD6\Projects\eMD6\core\driver，复制路径下的eMPL到自己工程
    在这里插入图片描述
代码移植
  • 需要修改的有三个文件，inv_mpu.h inv_mpu.c inv_mpu_dmp_motion_driver.c
首先是inv_mpu.h，在#define _INV_MPU_H_下添加define
#define _INV_MPU_H_

#define EMPL_TARGET_STM32F4
#define MPU6050
inv_mpu.c需要实现#if defined EMPL_TARGET_STM32F4下的宏定义，在上方注释有每个函数的接口定义
#if defined EMPL_TARGET_STM32F4
// #include "i2c.h"   
// #include "main.h"
// #include "log.h"
// #include "board-st_discovery.h"
#include "./i2c/bsp_i2c.h"  
#include "./systick/bsp_SysTick.h"

#define i2c_write   Soft_DMP_I2C_Write
#define i2c_read    Soft_DMP_I2C_Read 
#define delay_ms    mdelay
#define get_ms      get_tick_count
#define log_i       printf    
#define log_e       printf    
#define min(a,b) ((a<b)?a:b)
  • Soft_DMP_I2C_Write和Soft_DMP_I2C_Read可以参照如下代码修改
#define I2C_Direction_Transmitter ((uint8_t)0x00)
#define I2C_Direction_Receiver ((uint8_t)0x01)

uint8_t Soft_DMP_I2C_Write(uint8_t soft_dev_addr, uint8_t soft_reg_addr, uint8_t soft_i2c_len, unsigned char *soft_i2c_data_buf)
{
    uint8_t i, result = 0;
    i2c_Start();
    i2c_SendByte(soft_dev_addr << 1 | I2C_Direction_Transmitter);
    result = i2c_WaitAck();
    if (result != 0)
        return result;

    i2c_SendByte(soft_reg_addr);
    result = i2c_WaitAck();
    if (result != 0)
        return result;

    for (i = 0; i < soft_i2c_len; i++)
    {
        i2c_SendByte(soft_i2c_data_buf[i]);
        result = i2c_WaitAck();
        if (result != 0)
            return result;
    }
    i2c_Stop();
    return 0x00;
}

uint8_t Soft_DMP_I2C_Read(uint8_t soft_dev_addr, uint8_t soft_reg_addr, uint8_t soft_i2c_len, unsigned char *soft_i2c_data_buf)
{
    uint8_t result;

    i2c_Start();
    i2c_SendByte(soft_dev_addr << 1 | I2C_Direction_Transmitter);
    result = i2c_WaitAck();
    if (result != 0)
        return result;

    i2c_SendByte(soft_reg_addr);
    result = i2c_WaitAck();
    if (result != 0)
        return result;

    i2c_Start();
    i2c_SendByte(soft_dev_addr << 1 | I2C_Direction_Receiver);
    result = i2c_WaitAck();
    if (result != 0)
        return result;

    //
    while (soft_i2c_len)
    {
        if (soft_i2c_len == 1)
            *soft_i2c_data_buf = i2c_ReadByte(0);
        else
            *soft_i2c_data_buf = i2c_ReadByte(1);
        soft_i2c_data_buf++;
        soft_i2c_len--;
    }
    i2c_Stop();
    return 0x00;
}
  • mdelay和get_tick_count比较简单，通过滴答定时器累减和累加即可
void mdelay(unsigned long nTime)
{
  TimingDelay = nTime;
  while (TimingDelay != 0)
    ;
}

int get_tick_count(unsigned long *count)
{
  count[0] = g_ul_ms_ticks;
  return 0;
}
// 放入定时器中断
void TimingDelay_Decrement(void)
{
  if (TimingDelay != 0x00)
    TimingDelay--;
}

void TimeStamp_Increment(void)
{
  g_ul_ms_ticks++;
}
inv_mpu_dmp_motion_driver.c需要实现#if defined EMPL_TARGET_STM32F4下的宏定义
  • 还需要注意在该文件的630多行有个__no_operation();需要进行修改
// __no_operation();
    __ASM("nop");
相关变量与函数
  • 变量和函数有的是初始化使用，有的是主循环使用，自己根据情况选择放置位置
static unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;		// error
    return b;
}
unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
{

  unsigned short scalar;

  /*
     XYZ  010_001_000 Identity Matrix
     XZY  001_010_000
     YXZ  010_000_001
     YZX  000_010_001
     ZXY  001_000_010
     ZYX  000_001_010
   */

  scalar = inv_row_2_scale(mtx);
  scalar |= inv_row_2_scale(mtx + 3) << 3;
  scalar |= inv_row_2_scale(mtx + 6) << 6;

  return scalar;
}

static signed char gyro_orientation[9] = {1, 0, 0,
                                          0, 1, 0,
                                          0, 0, 1};

#define DEFAULT_MPU_HZ (20)
#define q30 1073741824.0f

uint8_t result;
unsigned char accel_fsr = 0;
unsigned short gyro_rate, gyro_fsr;
struct int_param_s int_param;

short gyro[3], accel_short[3], sensors;
unsigned char more;
long accel[3], quat[4], temperature;
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
float pitch = 0.0f, roll = 0.0f, yaw = 0.0f;
unsigned long sensor_timestamp;
主函数初始化
// I2C初始化
  i2c_GPIO_Config();

  printf("mpu 6050 test start\n");

  result = mpu_init(&int_param);
  if (result)
  {
    printf("one failed\n");
  }
  else
  {
    printf("one success\n");
  }

  // 使能陀螺仪和加速度
  mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
  // 使能FIFO，并设置采样率
  mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
  mpu_set_sample_rate(DEFAULT_MPU_HZ);
  // 获取采样率，存到gyro_rate
  mpu_get_sample_rate(&gyro_rate);
  // 获取量程，存到gyro_rate
  mpu_get_gyro_fsr(&gyro_fsr);
  mpu_get_accel_fsr(&accel_fsr);

  // 加载dmp固件
  dmp_load_motion_driver_firmware();
  // 设置方向矩阵
  dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));

  // 启用配置,六轴四元数计算，敲击检测， 屏幕旋转检测，原始加速度发送到FIFO，校准陀螺仪发送到FIFO，陀螺仪校准
  dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
                     DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
                     DMP_FEATURE_GYRO_CAL);

  // 设置向FIFO中输出数据的频率
  dmp_set_fifo_rate(DEFAULT_MPU_HZ);
  // 启动dmp
  mpu_set_dmp_state(1);
主函数循环
  while (1)
  {
    if (bsp_CheckTimer(KEY_TIMER))
    {
      dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more);
      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
        printf("pitch: %f, roll: %f, yaw: %f\n", roll, pitch, yaw);
      }
    }
  }

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/2130424.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

GD32/STM32启动过程

GD32/STM32启动过程

GD32/STM32启动过程 文章目录 GD32/STM32启动过程前言一、系统架构二、自举配置三、启动文件四、启动流程总结 前言 本文以STM32F407为例简单介绍其启动过程。 提示&#xff1a;以下是本篇文章正文内容&#xff0c;下面案例可供参考 一、系统架构 STM32F407的系统架构如图所…
阅读更多...
libyuv之linux编译

libyuv之linux编译

文章目录 一、下载源码二、编译源码三、注意事项1、银河麒麟系统&#xff08;aarch64&#xff09;&#xff08;1&#xff09;解决 armv8-adotprodi8mm 指令集支持问题&#xff08;2&#xff09;解决 armv9-asve2 指令集支持问题 一、下载源码 到GitHub网站下载https://github.…
阅读更多...
【JAVA】Undertow的使用及性能优化，以及Undertow与Tomcat的对比

【JAVA】Undertow的使用及性能优化，以及Undertow与Tomcat的对比

文章目录 Undertow优势Undertow使用Undertow性能优化线程池配置缓冲区和内存使用连接和请求超时禁用无关的设置编程式配置启用HTTP/2 Undertow与Tomcat的对比 更多相关内容可查看 当你进行Tomcat调优发现在怎么调都无法满足需求的时候&#xff0c;Undertow容器在性能和内存上都…
阅读更多...
盘点2024年大家都在用的短视频剪辑工具

盘点2024年大家都在用的短视频剪辑工具

你现在休息的时间是不是都靠短视频来消遣&#xff1f;看着看着你就会发现短视频制作好像我也可以了吧&#xff1f;这次我就介绍一些简单好操作的短视频剪辑工具。 1.FOXIT视频剪辑 连接直达>>https://www.pdf365.cn/foxitclip/ 短视频剪辑其实也不难&#xff0c;只需…
阅读更多...
MySql-表的内外连接

MySql-表的内外连接

文章目录 前言一、内连接二、外连接1.左外连接2.右外连接 前言 表的内外连接跟我们上节课讲的笛卡尔积很相似。 一、内连接 格式&#xff1a;select 字段 from 表1 inner join 表2 on 连接条件 and 其他条件&#xff1b; 例如&#xff1a;显示SMITH的名字和部门名称 二、外…
阅读更多...
RB-SQL：利用检索LLM框架处理大型数据库和复杂多表查询的NL2SQL

RB-SQL：利用检索LLM框架处理大型数据库和复杂多表查询的NL2SQL

NL2SQL的任务是将自然语言问题转换为SQL查询&#xff0c;以便从数据库中获取答案。现有LLM来指导SQL生成的方法在处理大型数据库和复杂多表查询时存在挑战&#xff0c;尤其是在处理冗余信息和提高提示工程效率方面。 (a) 利用大型语言模型&#xff08;LLM&#xff09;解决文本…
阅读更多...
ctfshow-web入门-sql注入（web249-web253）nosql 注入

ctfshow-web入门-sql注入（web249-web253）nosql 注入

目录 1、web249 2、web250 3、web251 4、wen252 5、web253 1、web249 开始nosql,flag在flag中 $user $memcache->get($id); 使用 Memcache 类的 get 方法从缓存中获取数据&#xff0c;$id 是传入的键&#xff0c;用于从缓存中获取与该键对应的值&#xff0c;结果存储…
阅读更多...
明基相机sd卡格式化还能恢复数据吗？可以这样操作

明基相机sd卡格式化还能恢复数据吗？可以这样操作

随着数字摄影的普及&#xff0c;相机SD卡已成为我们记录生活点滴的重要载体。然而&#xff0c;不小心将SD卡格式化&#xff0c;导致珍贵照片和视频瞬间消失&#xff0c;无疑是摄影爱好者的一大噩梦。面对这样的突发情况&#xff0c;许多朋友可能会感到无助与绝望。但实际上&…
阅读更多...
【计算机组成原理】概述+数制与编码

【计算机组成原理】概述+数制与编码

目录 计算机组成原理概述计算机基本组成计算机系统的层次结构计算机性能指标 数制与编码进位计数制及其相互转换数据的编码与表示校验码 计算机组成原理概述 计算机基本组成 运算器、控制器、存储器、输入输出设备 运算器&#xff1a;完成数据的暂存、变换、算术运算和逻辑…
阅读更多...
OpenCV结构分析与形状描述符（23）确定一个点是否位于多边形内的函数pointPolygonTest()的使用

OpenCV结构分析与形状描述符（23）确定一个点是否位于多边形内的函数pointPolygonTest()的使用

操作系统&#xff1a;ubuntu22.04 OpenCV版本&#xff1a;OpenCV4.9 IDE:Visual Studio Code 编程语言&#xff1a;C11 算法描述 进行点在轮廓内的测试。 该函数确定点是在轮廓内、轮廓外&#xff0c;还是位于一条边上&#xff08;或与顶点重合&#xff09;。它返回正值&…
阅读更多...
【可变参模板】基类参数包的展开

【可变参模板】基类参数包的展开

一、基类参数包的展开 1.1基类参数包的展开 C C C是一个支持多继承的语言&#xff0c;因此继承的类也可以是一个基类的参数包 注意继承的语法是 p u b l i c : c l a s s . . . public: class ... public:class...&#xff0c;需要补充 . . . ... ...作为基类参数包的标志。…
阅读更多...
Linux 驱动编写框架 并编译导入开发板

Linux 驱动编写框架 并编译导入开发板

向内核新加文件&#xff1a;例如 demo1.c 1. 创建并编辑新的文件 #include <linux/init.h> #include <linux/kernel.h> #include <linux/types.h> #include <linux/fs.h> #include <linux/module.h> #include <linux/kdev_t.h> #include …
阅读更多...
Qt控制开发板的LED

Qt控制开发板的LED

Qt控制开发板的LED 使用开发板的IO接口进行控制是嵌入式中非常重要的一点&#xff0c;就像冯诺依曼原理说的一样&#xff0c;一个计算机最起码要有输入输出吧&#xff0c;我们有了信息的接收和处理&#xff0c;那我们就要有输出。 我们在开发板上一般都是使用开发板的GPIO接口…
阅读更多...
vue3使用provide和inject传递异步请求数据子组件接收不到

vue3使用provide和inject传递异步请求数据子组件接收不到

前言 一般接口返回的格式是数组或对象&#xff0c;使用reactive定义共享变量 父组件传递 const data reactive([])// 使用settimout模拟接口返回 setTimeout(() > {// 将接口返回的数据赋值给变量Object.assign(data, [{ id: 10000 }]) }, 3000);provide(shareData, dat…
阅读更多...
02请求响应（简单参数）

02请求响应（简单参数）

一、操作目的 前端通过post/get请求&#xff0c;传递给后端简单的数据&#xff0c;后端接收后在控制台打印出来&#xff0c;并将结果返回给前端页面展示出来。&#xff08;这里我们用postman来模拟前端页面&#xff0c;而非真实的通过编写前端代码&#xff0c;通过浏览器来展示…
阅读更多...
Docker数据挂载本地目录

Docker数据挂载本地目录

docker内的数据映射可以不通过数据卷&#xff0c;直接映射到本地的目录。下面将以mysql容器示例&#xff0c;完成容器的数据映射。 注意&#xff1a;每一个不同的镜像&#xff0c;将来创建容器后内部有哪些目录可以挂载&#xff0c;可以参考DockerHubDocker Hub Container Ima…
阅读更多...
求树上任意两个点的距离lca

求树上任意两个点的距离lca

前言&#xff1a;一开始看到这个题目的时候&#xff0c;感觉就和lca有关&#xff0c;但是没有想到具体的公式 d d e p [ x ] d e p [ y ] − 2 ∗ d e p [ l c a ( x , y ) ] d dep[x] dep[y] - 2*dep[lca(x,y)] ddep[x]dep[y]−2∗dep[lca(x,y)] 并且我们这个题目还是一个…
阅读更多...
[数据集][目标检测]烟叶病害检测数据集VOC+YOLO格式612张3类别

[数据集][目标检测]烟叶病害检测数据集VOC+YOLO格式612张3类别

数据集格式&#xff1a;Pascal VOC格式YOLO格式(不包含分割路径的txt文件&#xff0c;仅仅包含jpg图片以及对应的VOC格式xml文件和yolo格式txt文件) 图片数量(jpg文件个数)&#xff1a;612 标注数量(xml文件个数)&#xff1a;612 标注数量(txt文件个数)&#xff1a;612 标注类别…
阅读更多...
【重学 MySQL】二十六、内连接和外连接

【重学 MySQL】二十六、内连接和外连接

【重学 MySQL】二十六、内连接和外连接 内连接&#xff08;INNER JOIN&#xff09;外连接&#xff08;OUTER JOIN&#xff09;总结 在MySQL中&#xff0c;内连接和外连接是两种常见的表连接方式&#xff0c;它们在处理多个表之间的关系时发挥着重要作用。 内连接&#xff08;I…
阅读更多...
光器件 -- 拉曼放大器（原理、分类和应用）

光器件 -- 拉曼放大器（原理、分类和应用）

拉曼工作原理 拉曼放大器基于受激拉曼散射效应&#xff08;SRS&#xff0c;Stimulated Raman Scattering&#xff09;&#xff0c;以传输光纤作为增益介质&#xff0c;将拉曼泵浦功率转移到C波段信号上进行放大。受激拉曼散射基本原理为&#xff1a;如果一个弱信号光与一个强泵…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023