Untitled
unknown
plain_text
a month ago
6.2 kB
2
Indexable
Never
#include <Arduino.h> #include <Adafruit_MPU6050.h> #include <Adafruit_Sensor.h> #include <Wire.h> Adafruit_MPU6050 mpu; // Variables to store calculated angles float pitch = 0; float roll = 0; float yaw = 0; // Arrays to store offsets float accelOffset[3] = {0, 0, 0}; // X, Y, Z offsets for accelerometer float gyroOffset[3] = {0, 0, 0}; // X, Y, Z offsets for gyroscope // Previous time in milliseconds unsigned long previousTime = 0; // Kalman filter variables float Q_angle = 0.001; float Q_bias = 0.003; float R_measure = 0.03; float angle = 0; // Reset the angle float bias = 0; // Reset bias float rate = 0; // Unbiased rate calculated from the rate and the calculated bias float P[2][2] = {{0, 0}, {0, 0}}; // Error covariance matrix float kalmanFilter(float newAngle, float newRate, float dt) { // Discrete Kalman filter time update equations - Time Update ("Predict") // Update xhat - Project the state ahead rate = newRate - bias; angle += dt * rate; // Update estimation error covariance - Project the error covariance ahead P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle); P[0][1] -= dt * P[1][1]; P[1][0] -= dt * P[1][1]; P[1][1] += Q_bias * dt; // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") // Calculate Kalman gain - Compute the Kalman gain float S = P[0][0] + R_measure; // Estimate error float K[2]; // Kalman gain - This is a 2x1 vector K[0] = P[0][0] / S; K[1] = P[1][0] / S; // Calculate angle and bias - Update estimate with measurement zk (newAngle) float y = newAngle - angle; // Angle difference angle += K[0] * y; bias += K[1] * y; // Calculate estimation error covariance - Update the error covariance float P00_temp = P[0][0]; float P01_temp = P[0][1]; P[0][0] -= K[0] * P00_temp; P[0][1] -= K[0] * P01_temp; P[1][0] -= K[1] * P00_temp; P[1][1] -= K[1] * P01_temp; return angle; } void calibrateMPU6050() { const int sampleSize = 1000; float accelSum[3] = {0, 0, 0}; // X, Y, Z sum for accelerometer float gyroSum[3] = {0, 0, 0}; // X, Y, Z sum for gyroscope for (int i = 0; i < sampleSize; i++) { sensors_event_t a, g, temp; mpu.getEvent(&a, &g, &temp); accelSum[0] += a.acceleration.x; accelSum[1] += a.acceleration.y; accelSum[2] += a.acceleration.z; gyroSum[0] += g.gyro.x; gyroSum[1] += g.gyro.y; gyroSum[2] += g.gyro.z; delay(3); } for (int i = 0; i < 3; i++) { accelOffset[i] = accelSum[i] / sampleSize; gyroOffset[i] = gyroSum[i] / sampleSize; } // Adjust the accelerometer Z offset to account for gravity (1g) accelOffset[2] -= 9.81; } void printMPUdata(){ //print_counter = micros(); Serial.print(F(">Pitch:")); Serial.println(pitch); Serial.print(F(">Row:")); Serial.println(roll); Serial.print(F(">Yaw:")); Serial.println(yaw); } void setup(void) { Serial.begin(115200); while (!Serial) delay(10); // will pause Zero, Leonardo, etc until serial console opens Serial.println("Adafruit MPU6050 test!"); // Try to initialize! if (!mpu.begin()) { Serial.println("Failed to find MPU6050 chip"); while (1) { delay(10); } } Serial.println("MPU6050 Found!"); mpu.setAccelerometerRange(MPU6050_RANGE_8_G); Serial.print("Accelerometer range set to: "); switch (mpu.getAccelerometerRange()) { case MPU6050_RANGE_2_G: Serial.println("+-2G"); break; case MPU6050_RANGE_4_G: Serial.println("+-4G"); break; case MPU6050_RANGE_8_G: Serial.println("+-8G"); break; case MPU6050_RANGE_16_G: Serial.println("+-16G"); break; } mpu.setGyroRange(MPU6050_RANGE_500_DEG); Serial.print("Gyro range set to: "); switch (mpu.getGyroRange()) { case MPU6050_RANGE_250_DEG: Serial.println("+- 250 deg/s"); break; case MPU6050_RANGE_500_DEG: Serial.println("+- 500 deg/s"); break; case MPU6050_RANGE_1000_DEG: Serial.println("+- 1000 deg/s"); break; case MPU6050_RANGE_2000_DEG: Serial.println("+- 2000 deg/s"); break; } mpu.setFilterBandwidth(MPU6050_BAND_5_HZ); Serial.print("Filter bandwidth set to: "); switch (mpu.getFilterBandwidth()) { case MPU6050_BAND_260_HZ: Serial.println("260 Hz"); break; case MPU6050_BAND_184_HZ: Serial.println("184 Hz"); break; case MPU6050_BAND_94_HZ: Serial.println("94 Hz"); break; case MPU6050_BAND_44_HZ: Serial.println("44 Hz"); break; case MPU6050_BAND_21_HZ: Serial.println("21 Hz"); break; case MPU6050_BAND_10_HZ: Serial.println("10 Hz"); break; case MPU6050_BAND_5_HZ: Serial.println("5 Hz"); break; } Serial.println("Calibrating MPU6050..."); calibrateMPU6050(); Serial.println("Calibration done!"); Serial.println(""); delay(100); } void loop() { // Get the current time unsigned long currentTime = millis(); float elapsedTime = (currentTime - previousTime) / 1000.0; // Convert to seconds previousTime = currentTime; // Get new sensor events with the readings sensors_event_t a, g, temp; mpu.getEvent(&a, &g, &temp); // Apply the offsets a.acceleration.x -= accelOffset[0]; a.acceleration.y -= accelOffset[1]; a.acceleration.z -= accelOffset[2]; g.gyro.x -= gyroOffset[0]; g.gyro.y -= gyroOffset[1]; g.gyro.z -= gyroOffset[2]; // Calculate pitch and roll using accelerometer data float accelPitch = atan2(a.acceleration.x, sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * 180.0 / PI; float accelRoll = atan2(-a.acceleration.y, sqrt(a.acceleration.x * a.acceleration.x + a.acceleration.z * a.acceleration.z)) * 180.0 / PI; pitch = accelPitch; roll = accelRoll; // Update yaw using gyroscope data yaw += g.gyro.z * elapsedTime; // Integrate to get yaw // Print out the values printMPUdata(); delay(100); }
Leave a Comment