while (1) { // 读取加速度和陀螺仪数据 MPU6050_Read_Accel(&ax, &ay, &az); MPU6050_Read_Gyro(&gx, &gy, &gz); for (i = 0; i < 5; i++) { MPU6050_Read_Accel(&ax[i], &ay[i], &az[i]); HAL_Delay(2); // 等待2ms再进行下一次采样 } // 对ax和ay进行中值滤波 ax[2] = Median_Filter(ax, 5); ay[2] = Median_Filter(ay, 5); az[2] = Median_Filter(az, 5); // 对ax进行时间积分 for (i = 0; i < 20; i++) { vx += ax[2] * 0.005; // 时间积分，假设采样间隔为0.005s } // 对ay进行平均处理 ay_sum = 0; for (i = 0; i < 20; i++) { ay_sum += ay[2]; HAL_Delay(2); // 等待2ms再进行下一次采样 } ay_sum /= 20; // 计算r r = vx * vx / ay_sum; // 计算a a = ay_sum - 2 * sqrt(1 - cos(vx*t/r)) * (vx/t); // 向PC输出a和滤波后的ax值 printf("a = %f, ax = %d,az = %d\r\n", a, ax[2],az[2]); HAL_Delay(10); } }修改代码把a以及数组形式输出

修改后的代码如下：

while (1) {
// 读取加速度和陀螺仪数据
MPU6050_Read_Accel(&ax, &ay, &az);
MPU6050_Read_Gyro(&gx, &gy, &gz);
for (i = 0; i < 5; i++) {
MPU6050_Read_Accel(&ax[i], &ay[i], &az[i]);
HAL_Delay(2); // 等待2ms再进行下一次采样
}
// 对ax和ay进行中值滤波
ax[2] = Median_Filter(ax, 5);
ay[2] = Median_Filter(ay, 5);
az[2] = Median_Filter(az, 5);
// 对ax进行时间积分
for (i = 0; i < 20; i++) {
vx += ax[2] * 0.005; // 时间积分，假设采样间隔为0.005s
}
// 对ay进行平均处理
ay_sum = 0;
for (i = 0; i < 20; i++) {
ay_sum += ay[2];
HAL_Delay(2); // 等待2ms再进行下一次采样
}
ay_sum /= 20;
// 计算r
r = vx * vx / ay_sum;
// 计算a
a = ay_sum - 2 * sqrt(1 - cos(vx*t/r)) * (vx/t);
// 向PC输出a和滤波后的ax值
printf("a = %f\n", a);
printf("ax = [");
for (i = 0; i < 5; i++) {
printf("%d", ax[i]);
if (i != 4) {
printf(", ");
}
}
printf("]\n");
printf("az = [");
for (i = 0; i < 5; i++) {
printf("%d", az[i]);
if (i != 4) {
printf(", ");
}
}
printf("]\n");
HAL_Delay(10);
}