移植MPU6050/9250的DMP官方库修改移植 DMP简单教程

只看该作者 · 2022-5-28 22:33

修改
基于原官方STM32F4部分略作修改，使之适用于STM32F1，编译器符号也应当从EMPL_TARGET_STM32F4改为EMPL_TARGET_STM32F1
/driver/eMPL/inv_mpu_dmp_motion_driver.c:627

只看该作者 · 2022-5-28 22:34

复制

#ifdef EMPL_TARGET_STM32F1

    __NOP();

#else

    __no_operation();

#endif

    // __no_operation();

只看该作者 · 2022-5-28 22:35

原文件中eMPL_outputs和hal_outputs在注册用于处理数据的回调函数时采用了相同的优先级，导致两个函数不能同时注册，导致eMPL_outputs.c和hal_outputs.c中的函数不能同时使用。所以修改了eMPL_outputs的优先级#define INV_PRIORITY_EMPL_OUTPUTS 899，经测试不影响结果，假如使用发现问题了，那就改回来吧。

只看该作者 · 2022-5-28 22:36

复制

#define INV_PRIORITY_MOTION_NO_MOTION          100

#define INV_PRIORITY_GYRO_TC                   150

#define INV_PRIORITY_QUATERNION_GYRO_ACCEL     200

#define INV_PRIORITY_QUATERNION_NO_GYRO        250

#define INV_PRIORITY_MAGNETIC_DISTURBANCE      300

#define INV_PRIORITY_HEADING_FROM_GYRO         350

#define INV_PRIORITY_COMPASS_BIAS_W_GYRO       375

#define INV_PRIORITY_COMPASS_VECTOR_CAL        400

#define INV_PRIORITY_COMPASS_ADV_BIAS          500

#define INV_PRIORITY_9_AXIS_FUSION             600

#define INV_PRIORITY_QUATERNION_ADJUST_9_AXIS  700

#define INV_PRIORITY_QUATERNION_ACCURACY       750

#define INV_PRIORITY_RESULTS_HOLDER            800

#define INV_PRIORITY_INUSE_AUTO_CALIBRATION    850

#define INV_PRIORITY_EMPL_OUTPUTS              899

#define INV_PRIORITY_HAL_OUTPUTS               900//原文件中均使用此优先级

#define INV_PRIORITY_GLYPH                     950

#define INV_PRIORITY_SHAKE                     975

#define INV_PRIORITY_SM                        1000





inv_register_data_cb(inv_generate_hal_outputs, INV_PRIORITY_HAL_OUTPUTS, INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);

inv_register_data_cb(inv_generate_eMPL_outputs, INV_PRIORITY_EMPL_OUTPUTS,INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);

只看该作者 · 2022-5-28 22:42

移植
inv_mpu.c和inv_mpu_dmp_motion_driver.c中有宏定义以供移植，原代码注释如下

只看该作者 · 2022-5-28 22:47

inv_mpu_dmp_motion_driver.c

只看该作者 · 2022-5-28 22:57

复制

/* The following functions must be defined for this platform:



** i2c_write(unsigned char slave_addr, unsigned char reg_addr,*



** unsigned char length, unsigned char const *data)*



** i2c_read(unsigned char slave_addr, unsigned char reg_addr,*



** unsigned char length, unsigned char *data)*



** delay_ms(unsigned long num_ms)*



** get_ms(unsigned long *count)*



*/

只看该作者 · 2022-5-28 22:58

inv_mpu.c

复制

/* The following functions must be defined for this platform:



** i2c_write(unsigned char slave_addr, unsigned char reg_addr,*



** unsigned char length, unsigned char const *data)*



** i2c_read(unsigned char slave_addr, unsigned char reg_addr,*



** unsigned char length, unsigned char *data)*



** delay_ms(unsigned long num_ms)*



** get_ms(unsigned long *count)*



** reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)*



** labs(long x)*



** fabsf(float x)*



** min(int a, int b)*



*/

只看该作者 · 2022-5-28 22:59

实际上需要提供的只有

i2c_write(unsigned char slave_addr, unsigned char reg_addr,unsigned char length, unsigned char const *data)

i2c_read(unsigned char slave_addr, unsigned char reg_addr,unsigned char length, unsigned char *data)

delay_ms(unsigned long num_ms)

get_ms(unsigned long *count)

只看该作者 · 2022-5-28 23:01

该部分具体代码如下：
#define i2c_write Sensors_I2C_WriteRegister

#define i2c_read Sensors_I2C_ReadRegister

#define delay_ms HAL_Delay

#define get_ms get_ms_user

只看该作者 · 2022-5-28 23:02

具体实现在porting文件夹中

只看该作者 · 2022-5-28 23:15

引脚
包含一个外部上升沿中断引脚和I2C的SDA、SCL引脚，共三个引脚

想要高频率的获取数据就用中断，不需要使用时关闭中断，等要数据时打开中断，高频率获取数据再滤波。

只看该作者 · 2022-5-28 23:17

编译
编译器设置符号：

EMPL_TARGET_STM32F1

EMPL

MPL_LOG_NDEBUG = 0/1 没有用官方python上位机的话设置为0

MPU6050你使用的型号

USE_DMP

REMOVE_LOGGING

只看该作者 · 2022-5-28 23:19

使用
请先完成移植

步骤为：

硬件初始化（MPU初始化）
MPL初始化（MPL 库是 InvenSense Motion Apps 专有算法的核⼼，由 Mllite 和 mpl ⽬录组成。）
设置MPL与DMP
调用函数获取数据

只看该作者 · 2022-5-28 23:22

注意：DMP传感器融合仅适用于+ -2000dps和Accel + -2G的陀螺仪（官方文档：DMP sensor fusion works only with gyro at ±2000dps and accel ±2G），大部分情况下我们都需要使用DMP（要不然也不用官方库了），所以设置并没有太多选择，照抄就完事了。除非你需要很大的量程，否则没有必要关闭DMP。

只看该作者 · 2022-5-28 23:22

前三步基本都是按照官方给的例子，封装成三个函数：

复制

uint8_t MPU6050_mpu_init(void);

uint8_t MPU6050_mpl_init(void);

uint8_t MPU6050_config(void);

在mpu6050_SL.c中

只看该作者 · 2022-5-28 23:23

重点是使用库中给出的函数，获取数据

官方的库中有三个.c文件提供了获取数据的接口，分别是mllite/results_holder.c、mllite/hal_outputs.c和eMPL-hal/eMPL_outputs.c。这三个文件获取都通过data_builder.c中的函数获取数据，所以也可以从data_builder.c中的函数获取原始数据自己处理~~（有现成的为什么要自己做呢）~~

只看该作者 · 2022-5-28 23:24

官方库处理数据的思路是将处理数据的函数以回调函数的方式存储起来，并在inv_error_t inv_execute_on_data(void)中统一调用，调用的顺序由注册时传入的优先级决定（本质就是拿一个数组把函数指针存起来，然后都调用一遍）。官方库的步骤是先注册启动函数，在调用启动函数来注册数据处理函数，再统一调用。

只看该作者 · 2022-5-28 23:25

例子如下（展示一下官方库处理数据的步骤，看一眼就行，不涉及使用）：

复制

inv_error_t inv_enable_hal_outputs(void)

{

    inv_error_t result;

    inv_init_hal_outputs();

    result = inv_register_mpl_start_notification(inv_start_hal_outputs);//注册启动函数，

    return result;

}



inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void))

{

    if (inv_start_cb.num_cb >= INV_MAX_START_CB)

        return INV_ERROR_INVALID_PARAMETER;



    inv_start_cb.start_cb[inv_start_cb.num_cb] = start_cb;//将函数指针存起来

    inv_start_cb.num_cb++;

    return INV_SUCCESS;

}



inv_error_t inv_start_mpl(void)

{

    INV_ERROR_CHECK(inv_execute_mpl_start_notification());//执行启动函数

    return INV_SUCCESS;

}



inv_error_t inv_start_hal_outputs(void)

{

    inv_error_t result;

    result =inv_register_data_cb(inv_generate_hal_outputs, INV_PRIORITY_HAL_OUTPUTS,

                             INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);//注册数据处理函数，同样将函数指针存起来，此处INV_PRIORITY_HAL_OUTPUTS是优先级，数组中函数指针按优先级排序

    return result;

}







inv_error_t inv_execute_on_data(void)

{

    inv_error_t result, first_error;

    int kk;

    int mode;



#ifdef INV_PLAYBACK_DBG

    if (inv_data_builder.debug_mode == RD_RECORD)

    {

        int type = PLAYBACK_DBG_TYPE_EXECUTE;

        fwrite(&type, sizeof(type), 1, inv_data_builder.file);

    }

#endif

    // Determine what new data we have

    mode = 0;

    if (sensors.gyro.status & INV_NEW_DATA)

        mode |= INV_GYRO_NEW;

    if (sensors.accel.status & INV_NEW_DATA)

        mode |= INV_ACCEL_NEW;

    if (sensors.compass.status & INV_NEW_DATA)

        mode |= INV_MAG_NEW;

    if (sensors.temp.status & INV_NEW_DATA)

        mode |= INV_TEMP_NEW;

    if (sensors.quat.status & INV_NEW_DATA)

        mode |= INV_QUAT_NEW;



    first_error = INV_SUCCESS;



    for (kk = 0; kk < inv_data_builder.num_cb; ++kk)

    {

        if (mode & inv_data_builder.process[kk].data_required)

        {

            result = inv_data_builder.process[kk].func(&sensors);//使用存起来的数据处理函数

            if (result && !first_error)

            {

                first_error = result;

            }

        }

    }



    inv_set_contiguous();



    return first_error;

}

只看该作者 · 2022-5-28 23:26

再上文提到过优先级冲突的事情，官方的相关部分代码如下：

复制

 for (kk = 0; kk < inv_data_builder.num_cb; ++kk)

    {

        if ((inv_data_builder.process[kk].func == func) || (inv_data_builder.process[kk].priority == priority))

        {

            return INV_ERROR_INVALID_PARAMETER; // fixme give a warning

        }

    }