開発環境
- Ubuntu 20.04 LTS
- STM32CubeIDE 1.9.0
- MPU6050 DMP ドライバー: motion_driver 5.1.3
- マイクロコントローラ: STM32 F103C8T6
motion_driver 5.1.3の移植
以下のファイルをmotion_driver-5.1.3/core/driver/eMPL/ディレクトリからコピーします。
- dmpKey.h
- inv_mpu.c
- inv_mpu_dmp_motion_driver.h
- dmpmap.h
- inv_mpu_dmp_motion_driver.c
- inv_mpu.h
inv_mpu.h と inv_mpu.c の修正
- inv_mpu.h にSTM32デバイス用のマクロ定義を追加
#define STM32_MPU6050 #define MPU6050 - inv_mpu.c でmsp430のマクロを新しく追加したSTM32_MPU6050に置き換え、msp430関連のヘッダファイルと構造体を削除
#if defined STM32_MPU6050 // msp430のヘッダファイルを削除 //#include "msp430.h" //#include "msp430_i2c.h" //#include "msp430_clock.h" //#include "msp430_interrupt.h" // HALライブラリを使用して再定義 #include "stm32f1xx_hal.h" #define i2c_write(mpuaddr, regaddr, dsize, pdata) HAL_I2C_Mem_Write(&hi2c1, (mpuaddr << 1), regaddr, I2C_MEMADD_SIZE_8BIT, pdata, dsize, HAL_MAX_DELAY) #define i2c_read(mpuaddr, regaddr, dsize, pdata) HAL_I2C_Mem_Read(&hi2c1, (mpuaddr << 1), regaddr, I2C_MEMADD_SIZE_8BIT, pdata, dsize, HAL_MAX_DELAY) #define delay_ms(ms) HAL_Delay(ms) #define get_ms(p) do { *p = HAL_GetTick();} while(0) // msp430関連の構造体を削除 //static inline int reg_int_cb(struct int_param_s *int_param) { ... } #define log_i(...) do {} while (0) #define log_e(...) do {} while (0) #define fabs fabsf #define min(a,b) ((a
MPU6050の初期化とデータ読み取り
1. 初期化関数: int MPU6050_Init(void);
int MPU6050_Init(void) {
int result;
struct int_param_s int_param;
struct hal_s hal = {0};
HAL_Delay(100);
result = mpu_init(&int_param);
if (result != 0) return -1;
result = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
if (result != 0) return -2;
result = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
if (result != 0) return -3;
result = mpu_set_sample_rate(DEFAULT_MPU_HZ);
if (result != 0) return -4;
result = dmp_load_motion_driver_firmware();
if (result != 0) return -5;
result = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
if (result != 0) return -6;
hal.dmp_features = 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;
result = dmp_enable_feature(hal.dmp_features);
if (result != 0) return -7;
result = dmp_set_fifo_rate(DEFAULT_MPU_HZ);
if (result != 0) return -8;
result = run_self_test();
if (result != 0) return -9;
result = mpu_set_dmp_state(1);
if (result != 0) return -10;
return 0;
}注意点: - mpu_initの前に100ミリ秒の遅延を入れること。 - inv_mpu.hからint_param_s構造体をコピー。 - サンプリングレート設定関数mpu_set_sample_rateのための定義: #define DEFAULT_MPU_HZ (100)。 - 方向設定関数dmp_set_orientationのためにinv_row_2_scaleとinv_orientation_matrix_to_scalar関数及びgyro_orientation[9]配列をコピー。 - 自己診断関数run_self_testをコピー。
2. データ読み取り関数: int MPU6050_DMP_GetData
int MPU6050_DMP_GetData(long *quat, double *pitch, double *roll, double *yaw) {
short gyro[3];
short accel[3];
double q[4] = {0.0f};
unsigned long timestamp;
short sensors;
unsigned char more;
while (dmp_read_fifo(gyro, accel, quat, ×tamp, &sensors, &more));
if (sensors & INV_WXYZ_QUAT) {
for (short i = 0; i < 4; i++)
q[i] = quat[i] / Q30;
*pitch = asin(-2 * q[1] * q[3] + 2 * q[0] * q[2]) * 57.3;
*roll = atan2(2 * q[2] * q[3] + 2 * q[0] * q[1], -2 * q[1] * q[1] - 2 * q[2] * q[2] + 1) * 57.3;
*yaw = atan2(2 * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]) * 57.3;
}
return 0;
}注意点: - FIFOのオーバーフローを避けるためにwhileループを使用。 - 四元数データをオイラー角に変換する際、double型を使用すること。