Board: Nano V3.0
I am using an MPU6050 to get roll and pitch. However, the serial monitor data stops at random time and I receive data again only after pressing the reset pin. Also the roll and pitch data I get is not correct, it should be between +-180' but at times I get 200' +
Code:
/* Get all possible data from MPU6050
* Accelerometer values are given as multiple of the gravity [1g = 9.81 m/s²]
* Gyro values are given in deg/s
* Angles are given in degrees
* Note that X and Y are tilt angles and not pitch/roll.
*/
#include "Wire.h"
#include <MPU6050_light.h>
MPU6050 mpu(Wire);
unsigned long lastTime, lastInterval;
float accelX, accelY, accelZ, // units m/s/s i.e. accelZ if often 9.8 (gravity)
gyroX, gyroY, gyroZ, // units dps (degrees per second)
gyroDriftX, gyroDriftY, // units dps
accRoll, accPitch, // units degrees (roll and pitch noisy, yaw not possible)
complementaryRoll, complementaryPitch;
void setup() {
Serial.begin(9600);
Wire.begin();
byte status = mpu.begin();
Serial.print(F("MPU6050 status: "));
Serial.println(status);
while (status != 0) {} // stop everything if could not connect to MPU6050
Serial.println(F("Calculating offsets, do not move MPU6050"));
delay(1000);
mpu.calcOffsets(true, true); // gyro and accelero
Serial.println("Done!\n");
}
void loop() {
mpu.update();
accelX = mpu.getAccX();
accelY = mpu.getAccY();
accelZ = mpu.getAccZ();
gyroX = mpu.getGyroX();
gyroY = mpu.getGyroY();
gyroZ = mpu.getGyroZ();
unsigned long currentTime = micros();
lastInterval = currentTime - lastTime; // expecting this to be ~104Hz +- 4%
lastTime = currentTime;
doCalculations();
Serial.println("Roll=");
Serial.println(complementaryRoll);
Serial.print("Pitch=");
Serial.print(complementaryPitch);
delay(10);
}
void doCalculations() {
accRoll = atan2(accelY, accelZ) * 180 / M_PI;
accPitch = atan2(-accelX, sqrt(accelY * accelY + accelZ * accelZ)) * 180 / M_PI;
float lastFrequency = (float)1000000.0 / lastInterval;
complementaryRoll = complementaryRoll + ((gyroX - gyroDriftX) / lastFrequency);
complementaryPitch = complementaryPitch + ((gyroY - gyroDriftY) / lastFrequency);
complementaryRoll = 0.98 * complementaryRoll + 0.02 * accRoll;
complementaryPitch = 0.98 * complementaryPitch + 0.02 * accPitch;
}