I’m trying to make a project in sensor fusion where I use a accel and gyro to get my position in space, but before I get there I’m trying to just read the sensor values, I’ve wrote my own code, but also used other peoples and in all cases the value in Z doesn’t change. I’ve read about yaw drift, but i believe it’s something that happens with the gyro not the accel.
#include <Wire.h>
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;
long gyroXCalli = 0, gyroYCalli = 0, gyroZCalli = 0;
long gyroXPresent = 0, gyroYPresent = 0, gyroZPresent = 0;
long gyroXPast = 0, gyroYPast = 0, gyroZPast = 0;
float rotX, rotY, rotZ;
float angelX = 0, angelY = 0, angelZ = 0;
long timePast = 0;
long timePresent = 0;
void setup() {
Serial.begin(9600);
Wire.begin();
setUpMPU();
callibrateGyroValues();
timePresent = millis();
}
void loop() {
// Serial.print(" Accel (g)");
// readAndProcessAccelData();
// readAndProcessGyroData();
readAndProcessAccelData();
getGyroValues();
printData();
delay(500);
}
void setUpMPU() {
// power management
Wire.beginTransmission(0b1101000); // Start the communication by using address of MPU
Wire.write(0x6B); // Access the power management register
Wire.write(0b00000000); // Set sleep = 0
Wire.endTransmission(); // End the communication
// configure gyro
Wire.beginTransmission(0b1101000);
Wire.write(0x1B); // Access the gyro configuration register
Wire.write(0b00000000);
Wire.endTransmission();
// configure accelerometer
Wire.beginTransmission(0b1101000);
Wire.write(0x1C); // Access the accelerometer configuration register
Wire.write(0b00000000);
Wire.endTransmission();
}
void callibrateGyroValues() {
for (int i=0; i<5000; i++) {
getGyroValues();
gyroXCalli = gyroXCalli + gyroXPresent;
gyroYCalli = gyroYCalli + gyroYPresent;
gyroZCalli = gyroZCalli + gyroZPresent;
}
gyroXCalli = gyroXCalli/5000;
gyroYCalli = gyroYCalli/5000;
gyroZCalli = gyroZCalli/5000;
}
void readAndProcessAccelData() {
Wire.beginTransmission(0b1101000);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0b1101000,6);
while(Wire.available() < 6);
accelX = Wire.read()<<8|Wire.read();
accelY = Wire.read()<<8|Wire.read();
accelZ = Wire.read()<<8|Wire.read();
processAccelData();
}
void processAccelData() {
gForceX = accelX/16384.0;
gForceY = accelY/16384.0;
gForceZ = accelZ/16384.0;
}
//void readAndProcessGyroData() {
// gyroXPast = gyroXPresent; // Assign Present gyro reaging to past gyro reading
// gyroYPast = gyroYPresent; // Assign Present gyro reaging to past gyro reading
// gyroZPast = gyroZPresent; // Assign Present gyro reaging to past gyro reading
// timePast = timePresent; // Assign Present time to past time
// timePresent = millis(); // get the current time in milli seconds, it is the present time
//
// getGyroValues(); // get gyro readings
// getAngularVelocity(); // get angular velocity
// calculateAngle(); // calculate the angle
//}
void getGyroValues() {
Wire.beginTransmission(0b1101000); // Start the communication by using address of MPU
Wire.write(0x43); // Access the starting register of gyro readings
Wire.endTransmission();
Wire.requestFrom(0b1101000,6); // Request for 6 bytes from gyro registers (43 - 48)
while(Wire.available() < 6); // Wait untill all 6 bytes are available
gyroXPresent = Wire.read()<<8|Wire.read(); // Store first two bytes into gyroXPresent
gyroYPresent = Wire.read()<<8|Wire.read(); // Store next two bytes into gyroYPresent
gyroZPresent = Wire.read()<<8|Wire.read(); //Store last two bytes into gyroZPresent
getAngularVelocity();
}
void getAngularVelocity() {
rotX = gyroXPresent / 131.0;
rotY = gyroYPresent / 131.0;
rotZ = gyroZPresent / 131.0;
}
void printData() {
Serial.println("Gyro (deg/sec)");
Serial.print(" X=");
Serial.print(rotX);
Serial.print(" Y=");
Serial.println(rotY);
Serial.print("Angle (deg)");
Serial.print(" Z=");
Serial.println(rotZ);
Serial.println("Acceleration (g)");
Serial.print(" X=");
Serial.print(gForceX);
Serial.print(" Y=");
Serial.print(gForceY);
Serial.print(" Z=");
Serial.println(gForceZ);
}