Arduino连接两个MPU6050测算姿态角

原创

已于 2023-06-01 15:38:28 修改 · 1.2k 阅读

标签

#算法 #mcu

于 2023-06-01 11:55:58 首次发布

Arduino连接两个MPU6050测算姿态角

单个MPU6050 连接

连线

在这里插入图片描述
采用卡尔曼滤波器，对加速度计进行修正。

//连线方法
//MPU-UNO
//VCC-VCC
//GND-GND
//SCL-A5
//SDA-A4
//INT-2 (Optional)

#include <Adafruit_MPU6050.h>
#include <Adafruit_sensor.h>
#include <Wire.h>
#include<Kalman.h>

Kalman.h 库可以通过IDE自带的库搜索下载。或者git：https://github.com/TKJElectronics/KalmanFilter
Adafruit_MPU6050 IDE自带，如果没自带用自带的库搜索下载。

完整代码

#include <Adafruit_MPU6050.h>
#include <Adafruit_sensor.h>
#include <Wire.h>
#include<Kalman.h>
const int RESET_PIN = 2; // 定义复位引脚为 D2
Adafruit_MPU6050 mpu;

float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float AccErrorX, AccErrorY, AccErrorZ,GyroErrorX, GyroErrorY, GyroErrorZ = 0;
float AccSumX, AccSumY, AccSumZ,GyroSumX, GyroSumY, GyroSumZ = 0;
int times = 0;
float fLastRoll = 0.0f; //上一次滤波得到的Roll角
float fLastPitch = 0.0f; //上一次滤波得到的Pitch角
unsigned long nLastTime = 0; //上一次读数的时间
Kalman kalmanRoll; //Roll角滤波器
Kalman kalmanPitch; //Pitch角滤波器
Kalman kalmanYaw; //Yaw角滤波器
void setup() {
   
   
  // put your setup code here, to run once:
  Serial.begin(115200);
  // initialize mpu
  if (!mpu.begin()) {
   
   
    Serial.println("Failed to start MPU6050");
    while (1) {
   
   
      delay(10);
    }
  } else {
   
   
    Serial.println("Start MPU6050");
  }
  // display MPU6050 settings
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_44_HZ);
  calculate_IMU_error();
  pinMode(2,OUTPUT);
}

void calculate_IMU_error()
{
   
   
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  int c = 0;
  int times_c =200;
  while(c<times_c){
   
   
    AccX = a.acceleration.x;
    AccY = a.acceleration.y;
    AccZ = a.acceleration.z;
    AccErrorX+=AccX;AccErrorY+=AccY;AccErrorZ+=AccZ;
    GyroX = g.gyro.x;
    GyroY = g.gyro.y;
    GyroZ = g.gyro.z;
    GyroErrorX+=GyroX;GyroErrorY+=GyroY;GyroErrorZ+=GyroZ;
    delay(0.001);c++;
  }
  AccErrorX = AccErrorX/times_c; AccErrorY = AccErrorY/times_c; AccErrorZ = AccErrorZ/times_c;
  GyroErrorX = GyroErrorX/times_c; GyroErrorY = GyroErrorY/times_c; GyroErrorZ = GyroErrorZ/times_c;
}
//均值滤波
void avg()
{
   
   
  int d = 0;
  int times_d = 5;
  AccSumX=0;AccSumY=0;AccSumZ=0;
  GyroSumX=0;GyroSumY=0;GyroSumZ=0;
  while(d<times_d)
  {
   
   
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    AccX = a.acceleration.x;
    AccY = a.acceleration.y;
    AccZ = a.acceleration.z;
    GyroX = g.gyro.x;
    GyroY = g.gyro.y;
    GyroZ = g.gyro.z;
    AccSumX+=AccX;AccSumY+=AccY;AccSumZ+=AccZ;
    GyroSumX+=GyroX;GyroSumY+=GyroY;GyroSumZ+=GyroZ;
    delay(0.001);d++;
  }
  AccX = AccSumX/times_d+AccErrorX;AccY = AccSumY/times_d+AccErrorY;AccZ = AccSumZ/times_d+AccErrorZ;
  GyroX=GyroSumX/times_d+GyroErrorX;GyroY=GyroSumY/times_d+GyroErrorY;GyroZ=GyroSumZ/times_d+GyroErrorZ;
}
//算得Roll角
float GetRoll(float AccX,float AccZ, float fNorm) {
   
   
  float fNormXZ = sqrt(AccX * AccX + AccZ * AccZ);
  float fCos = fNormXZ / fNorm;
  return acos(fCos) * fRad2Deg;
}

//算得Pitch角
float GetPitch(float AccY,float AccZ, float fNorm) {
   
   
  float fNormYZ = sqrt(AccY * AccY + AccZ * AccZ);
  float fCos = fNormYZ / fNorm;
  return acos(fCos) * fRad2Deg;
}
//算得yaw角
float GetYaw(float AccX,float AccY, float fNorm) {
   
   
  float fNormXY = sqrt(AccX * AccX + AccY * AccY);
  float fCos = fNormXY

最低0.47元/天解锁文章