I'm struggling to figure out why this simple MPU6050 test code does not work. My hardware seems to work based on an alternate library, but that library does not provide pitch/roll/yaw. Any help would be much appreciated.
#include "Simple_MPU6050.h"
#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board
#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)
#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW
Simple_MPU6050 mpu;
ENABLE_MPU_OVERFLOW_PROTECTION();
Quaternion q;
VectorFloat gravity;
float ypr[3] = { 0, 0, 0 };
float xyz[3] = { 0, 0, 0 };
void Scope1 () { PORTD |= bit (4); }
void Scope0 () { PORTD &= ~bit (4); }
void LEDgrn () { PORTB |= 1; PORTB &= ~2; }
void LEDred () { PORTB |= 2; PORTB &= ~1; }
void LEDoff () { PORTB |= 1; PORTB |= 2; }
void Piezo () { PORTD ^= (1 << 2); /* PD2 */ }
void Beep (int per, int dur) { for (int j = 1; j < dur; j++) { Piezo (); delayMicroseconds (per); } }
void Beep1 (int n) { while (n--) { Beep (170,300); delay (150); } }
void BeepD () { Beep1 (1); delay (200); }
#define spamtimer(t) for (static uint32_t SpamTimer; (uint32_t)(millis() - SpamTimer) >= (t); SpamTimer = millis())
void print_Values (int16_t *gyro, int16_t *accel, int32_t *quat, uint32_t *timestamp) {
uint8_t Spam_Delay = 100;
// Next line from lib example but does not work for me.
// spamtimer(Spam_Delay) { // non blocking delay before printing again. This skips the following code when delay time (ms) hasn't been met
mpu.GetQuaternion(&q, quat);
mpu.GetGravity(&gravity, &q);
mpu.GetYawPitchRoll(ypr, &q, &gravity);
mpu.ConvertToDegrees(ypr, xyz);
// No serial (because programming custom board using Arduino as ISP) so using scope.
Beep (170,100);
int Deg = ypr[2];
Scope1 (); delay (10 + abs (Deg)); Scope0 (); } // Shows 10 ms pulse, unaffected by angle.
void setup() {
DDRB |= bit(0); // B0 OUT LEDA
DDRB |= bit(1); // B1 OUT LEDB
DDRD |= bit(2); // D2 OUT PIEZO
DDRD |= bit(4); // D4 OUT ElevServoPin
BeepD (); LEDgrn ();
BeepD (); LEDred ();
BeepD (); Wire.begin();
BeepD (); mpu.SetAddress(MPU6050_ADDRESS_AD0_LOW).CalibrateMPU().load_DMP_Image();
BeepD (); mpu.on_FIFO(print_Values);
BeepD (); LEDgrn (); }
void loop() { mpu.dmp_read_fifo(); }