姆ST-MYD-YA15XC开发板传感器应用实例开发笔记

/*0x01:Enable gyro DLPF 1000DPS*/

DEV_I2C_WriteByte( REG_ADD_GYRO_CONFIG_1, REG_VAL_BIT_GYRO_DLPCFG_6 | REG_VAL_BIT_GYRO_FS_1000DPS |

REG_VAL_BIT_GYRO_DLPF);

/*0x11:LSB for ACCEL sample rate div 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0])*

/

DEV_I2C_WriteByte( REG_ADD_ACCEL_SMPLRT_DIV_2, 0x07);

/*0x14: ACCEL_CONFIG 2g DLPF*/

DEV_I2C_WriteByte( REG_ADD_ACCEL_CONFIG, REG_VAL_BIT_ACCEL_DLPCFG_6 | REG_VAL_BIT_ACCEL_FS_2g | REG_V

AL_BIT_ACCEL_DLPF);

/* user bank 0 register */

DEV_I2C_WriteByte( REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0);

DEV_Delay_ms(100);

/* offset 瞬时时测得的原始天内据*/

invmsICM20948GyroOffset();

/*设置磁力计住址，使能读*/

invmsICM20948MagCheck();

invmsICM20948WriteSecondary( I2C_ADD_ICM20948_AK09916|I2C_ADD_ICM20

948_AK09916_WRITE,REG_ADD_MAG_CNTL2, REG_VAL_MAG_MODE_20HZ);

return;

}

ICM20948 射频四元天内迭代设计，在 ICM20948/IMU/IMU.c 档案之中：

/**

* @brief Updata attitude and heading

* @param ax: accelerometer X

* @param ay: accelerometer Y

* @param az: accelerometer Z

* @param gx: gyroscopes X

* @param gy: gyroscopes Y

* @param gz: gyroscopes Z

* @param mx: magnetometer X

* @param my: magnetometer Y

* @param mz: magnetometer Z

* @retval None

*/

void IMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float m

x, float my, float mz)

{

float norm;

float hx, hy, hz, bx, bz;

float vx, vy, vz, wx, wy, wz;

float exInt = 0.0, eyInt = 0.0, ezInt = 0.0;

float ex, ey, ez, halfT = 0.024f;

/*四元天内相关浮点运算，回转矩阵*/

float q0q0 = q0 * q0;

float q0q1 = q0 * q1;

float q0q2 = q0 * q2;

float q0q3 = q0 * q3;

float q1q1 = q1 * q1;

float q1q2 = q1 * q2;

float q1q3 = q1 * q3;

float q2q2 = q2 * q2;

float q2q3 = q2 * q3;

float q3q3 = q3 * q3;

/*将摩擦力计精确测量个天内再生为三维单位矩阵(归一化)*/

norm = invSqrt(ax * ax + ay * ay + az * az);

ax = ax * norm;

ay = ay * norm;

az = az * norm;

/*将磁射频精确测量个天内再生为三维单位矩阵(归一化)*/

norm = invSqrt(mx * mx + my * my + mz * mz);

mx = mx * norm;

my = my * norm;

mz = mz * norm;

/*hx，hy，hz 是地理周围环境球面下的磁射频个天内，可以有机体球面下的 mx，my，

mz 左乘回转矩阵取得*/

hx = 2 * mx * (0.5f - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q

3 + q0q2);

hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5f - q1q1 - q3q3) + 2 * mz * (q2q

3 - q0q1);

hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5f - q1q

1 - q2q2);

bx = sqrt((hx * hx) + (hy * hy));

bz = hz;

/*用四元天内透露三轴重力分量 vx，vy,vz,钉据此地理周围环境球面下的重力摩擦力个天内，右乘回转

矩阵即可取得此时机体球面下的重力摩擦力估计个天内*/

// estimated direction of gravity and flux (v and w)

vx = 2 * (q1q3 - q0q2);

vy = 2 * (q0q1 + q2q3);

vz = q0q0 - q1q1 - q2q2 + q3q3;

wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);

wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);

wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2);

/*钉据上面所求的个天内求飞轮微分值个天内，

矩阵间的值 ex，ey，ez 是用矩阵积(外积、钝乘)透露，

该值矩阵仍位于机体球面之中

*/

// error is sum of cross product between reference direction of fields and di

rection measured by sensors

ex = (ay * vz - az * vy) + (my * wz - mz * wy);

ey = (az * vx - ax * vz) + (mz * wx - mx * wz);

ez = (ax * vy - ay * vx) + (mx * wy - my * wx);

/*利用所得的值个天内更新飞轮的精确测量个天内，使用 PI 迭代完成控制

式之中，实例 Ki、Kp 用来控制更新飞轮值速度

取得属于自己飞轮个天内 gx、gy、gz

*/

if(ex != 0.0f && ey != 0.0f && ez != 0.0f)

{

exInt = exInt + ex * Ki * halfT;

eyInt = eyInt + ey * Ki * halfT;

ezInt = ezInt + ez * Ki * halfT;

gx = gx + Kp * ex + exInt;

gy = gy + Kp * ey + eyInt;

gz = gz + Kp * ez + ezInt;

}

/*利用更新后的飞轮个天内 gx、gy、gz 更新四元天内*/

q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;

q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;

q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;

q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;

/*将更新后的四元天内再生为三维单位矩阵(归一化)*/

norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

q0 = q0 * norm;

q1 = q1 * norm;

q2 = q2 * norm;

q3 = q3 * norm;

}

飞轮、摩擦力、磁力计、欧拉角只读，在

ICM20948/invMotionSensor.c 档案之中：

extern void invMSAccelRead(int16_t* ps16AccelX, int16_t* ps16AccelY, int16_t* ps

16AccelZ)

{

if( (genCurrentSensorType != INVMS_EN_SENSOR_TYPY_MAX) &&

(ps16AccelX != NULL) &&

(ps16AccelY != NULL) &&

(ps16AccelZ != NULL) )

{

gsstSensorList[genCurrentSensorType].pFunAccelRead(ps16AccelX, ps16Acce

lY, ps16AccelZ);

}

return;

}

extern void invMSGyroRead(int16_t* ps16GyroX, int16_t* ps16GyroY, int16_t* ps16

GyroZ)

{

if( (genCurrentSensorType != INVMS_EN_SENSOR_TYPY_MAX) &&

(ps16GyroX != NULL) &&

(ps16GyroY != NULL) &&

(ps16GyroZ != NULL) )

{

gsstSensorList[genCurrentSensorType].pFunGyroRead(ps16GyroX, ps16GyroY, ps16GyroZ);

}

return;

}

extern void invMSMagRead(int16_t* ps16MagnX, int16_t* ps16MagnY, int16_t* ps

16MagnZ)

{

if( (genCurrentSensorType != INVMS_EN_SENSOR_TYPY_MAX) &&

(ps16MagnX != NULL) &&

(ps16MagnY != NULL) &&

(ps16MagnZ != NULL) )

{

gsstSensorList[genCurrentSensorType].pFunMagRead(ps16MagnX, ps16Mag

nY, ps16MagnZ);

}

return;

}

/**

* @brief Get Yaw Pitch Roll

* @param None

* @retval None

*/

void IMU_GetYawPitchRoll(float *Angles)

{

IMU_GetQuater();

Angles[1] = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch

Angles[2] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 5

7.3; // roll

Angles[0] = atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) *

57.3;

}

在主变量之中，突然间的调用只读变量，对原始天内据完成处理过程然后打印：

while (1)

{

/* USER CODE END WHILE */

/* USER CODE BEGIN 3 */

IMU_GetYawPitchRoll(angles);

printf(" /-------------------------------------------------------------/

");

printf(" Roll: %.2f Pitch: %.2f Yaw: %.2f ",angles[2],angles[1],an

gles[0]);

printf(" Acceleration: X: %d Y: %d Z: %d ",accel[0],accel[1],acc

el[2]);

printf(" Gyroscope: X: %d Y: %d Z: %d ",gyro[0],gyro[1],gyro

[2]);

printf(" Magnetic: X: %d Y: %d Z: %d ",magn[0],magn[1],magn

[2]);

DEV_Delay_ms(500);

}

/* USER CODE END 3 */

}

delay = 0;

}

if(SUCCESS == over_flag){

5.4. 测试

5.4.1.原型车模式叫停 m4 固件

叫停开发板，并叫停 m4 固件，如下：

root@myir-ya151c-t-4e512d:~# cp ICM20948_CM4.elf /lib/firmware/

root@myir-ya151c-t-4e512d:~# echo ICM20948_CM4.elf> /sys/class/remoteproc

/remoteproc0/firmware

root@myir-ya151c-t-4e512d:~# echo start> /sys/class/remoteproc/remoteproc0/

state

[ 74.763629] remoteproc remoteproc0: powering up m4

[ 74.772761] remoteproc remoteproc0: Booting fw image ICM20948_CM4.elf, siz

e 2649856

[ 74.784968] remoteproc remoteproc0: header-less resource table

[ 74.789341] remoteproc remoteproc0: no resource table found for this firmware

[ 74.800724] remoteproc remoteproc0: header-less resource table

[ 74.807706] remoteproc remoteproc0: remote processor m4 is now up

5.4.2. 天内据接收

打开 sscom，可以看到串口能打印 ICM20948 采集到的原始天内据，如下三幅所示：

三幅 5-2.原始天内据接收

。

太原皮肤病权威医院
天津牛皮癣专科医院哪家好
阳萎治疗
儿童便秘
生殖整形
慢性支气管炎咳嗽老不好怎么办
急支糖浆的功效和作用
药品库