Short update. Seems to be my code, or the libraries I'm using together, not my MPU6050 chip. Still not knowing how to fix that, neither if I'll be able to fix it decently.
I've now used Arduino Scheduler, then tried TaskScheduler. Updates for my display also (SSD1327 128x128 OLED). Issue is the same.
If I place a resetFIFO(), occasionnally only some values get corrupeted. Without it and with the below code, I get outputs which quickly seem fully random out of the MPU6050 (laying still and flat on my desk):
MPU6050 connection successfulChecking hardware revision...
Revision @ user[16][6] = A5
Resetting memory bank selection to 0...
>***......>......
// X Accel Y Accel Z Accel X Gyro Y Gyro Z Gyro
//OFFSETS -3138, 647, 1559, 100, -7, 82
-0.01, roll max 0.01 0.01/0.00
0.00, roll max 0.01 0.01/0.01
-146.12, roll max 178.93 178.93/178.46
-176.90, roll max 178.93 178.93/178.46
-147.69, roll max 178.93 178.93/178.46
0.00, roll max 179.99 179.93/179.99
119.07, roll max 179.99 179.93/179.99
-137.14, roll max 179.99 179.93/179.99
Here only -0.01, roll max 0.01 0.01/0.00 is acceptable. From all my tests, the 0.00 lines seem to me also corrupted.
With the extra resetFIFO() line after every mpu.getFIFOBytes(fifoBuffer, packetSize);, it almost works. But the MPU6050 plus corresponding library and codes seem to work like expected only if I do disable the SSD1327 display updates (commenting out u8g2.sendBuffer();, which pushes an 8k RAM buffer to the OLED). 8K shall not the issue, the Teensy 3.2 has 64k RAM.
resetFIFO() only sends a message to the MPU...
void MPU6050::resetFIFO() {
I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true);
}
So I may have a SSD1327 driver issue, maybe RAM allocation or RAM corruption issue in my tasked or threaded environment. The confusing part is that my issue causes the MPU collected data to vary fastly between -180/+180°:
119.07, roll max 179.99 179.93/179.99
-137.14, roll max 179.99 179.93/179.99
So far, I believe my own code is almost clean. This is with TaskScheduler, but I have the same behaviour with Arduino Scheduler (for threads and yields), it seems that what is correctly gathered from MPU6050 gets corrupted on display/OLED updates:
// Debug and Test options
#define _DEBUG_
//#define _TEST_
#ifdef _DEBUG_
#define _PP(a) Serial.print(a);
#define _PL(a) Serial.println(a);
#else
#define _PP(a)
#define _PL(a)
#endif
#include <TaskScheduler.h>
#define INTERVAL_DISPLAY (333L)
Scheduler runner;
#include <SPI.h>
#define __U8G2LIB_
#define CS_SSD1327 10
#define DC_SSD1327 6
#define RST_SSD1327 8
#ifdef __U8G2LIB_
#include <U8g2lib.h>
U8G2_SSD1327_WS_128X128_F_4W_HW_SPI u8g2(U8G2_R0, CS_SSD1327, DC_SSD1327, RST_SSD1327);
#else
#include "SSD1327.h"
SSD1327 disp(CS_SSD1327, DC_SSD1327, RST_SSD1327);
#endif
#include <Wire.h>
#include "MPU6050_6Axis_MotionApps20.h"
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
MPU6050 mpu;
#define INTERRUPT_PIN 15
#define DATA_SAMPLES_ROLL 500
volatile float max_roll[DATA_SAMPLES_ROLL];
volatile int max_roll_recent_pointer;
volatile float max_roll_trip_left, max_roll_trip_right;
// Callback methods prototypes
void tDisplayCallback();
void tMPUSetupCallback();
void tMPUInterruptCallback();
unsigned long tDisplay_interval = INTERVAL_DISPLAY;
// Tasks
Task tDisplay(tDisplay_interval, TASK_FOREVER, &tDisplayCallback, &runner, true);
Task tMPUSetup(TASK_IMMEDIATE, TASK_ONCE, &tMPUSetupCallback, &runner, true);
Task tMPUInterrupt(TASK_IMMEDIATE, TASK_ONCE, &tMPUInterruptCallback, &runner, false);
void dmpDataReady() {
tMPUInterrupt.restart();
}
void tMPUInterruptCallback() {
uint16_t fifoCount = 0; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
static int sample = 0;
uint8_t mpuIntStatus;
while (fifoCount < packetSize)
fifoCount = mpu.getFIFOCount();
mpuIntStatus = mpu.getIntStatus();
if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
mpu.getFIFOBytes(fifoBuffer, packetSize);
//mpu.resetFIFO();
// Keep 1/4 which is a rate of 25Hz
float ypr[3]; // [yaw, roll, roll] yaw/roll/roll container and gravity vector
if (!(sample++ % 4)) {
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
max_roll[max_roll_recent_pointer++] = abs(ypr[1]);
if (max_roll_recent_pointer == DATA_SAMPLES_ROLL)
max_roll_recent_pointer = 0;
if (ypr[1] > 0 )
max_roll_trip_right = max(ypr[1], max_roll_trip_right);
if (ypr[1] < 0)
max_roll_trip_left = max(abs(ypr[1]), max_roll_trip_left);
}
if (!(sample % 100)) {
float roll = 0;
for (int i = 0; i < DATA_SAMPLES_ROLL; i++)
roll = max(roll, max_roll[i]);
_PP(ypr[1] * 180/M_PI);
_PP(", roll max ");
_PP(roll * 180/M_PI);
_PP(" ");
_PP(max_roll_trip_left * 180/M_PI);
_PP("/");
_PL(max_roll_trip_right * 180/M_PI);
}
}
}
int displayBlink;
void tDisplayCallback() {
if (tDisplay.isFirstIteration()) {
#ifdef __U8G2LIB_
u8g2.begin();
#else
#endif
}
#ifdef __U8G2LIB_
u8g2.clearBuffer();
u8g2.setFont(u8g2_font_logisoso16_tf);
if (displayBlink++ % 2)
u8g2.drawStr((128 - u8g2.getStrWidth("Active...")) / 2, (128 - u8g2.getMaxCharHeight() + u8g2.getMaxCharHeight()) / 2,
"Active...");
u8g2.sendBuffer();
#else
disp.clearBuffer();
if (displayBlink++ % 2) {
disp.drawString(16, 16, "Hello", 0xF, 32);
disp.drawString(16, 48, "World!", 0xF, 32);
}
disp.writeFullBuffer();
#endif
}
void tMPUSetupCallback() {
uint8_t devStatus;
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
_PP(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(61);
mpu.setYGyroOffset(-28);
mpu.setZGyroOffset(-6);
mpu.setZAccelOffset(1471);
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
mpu.setDMPEnabled(true);
packetSize = mpu.dmpGetFIFOPacketSize();
max_roll_trip_left = 0;
max_roll_trip_right = 0;
max_roll_recent_pointer = 0;
for (int i = 0; i < DATA_SAMPLES_ROLL; i++)
max_roll[i] = 0;
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
}
}
void setup () {
SPI.setSCK(14);
SPI.begin();
Wire.begin();
Wire.setClock(400000L);
#if defined(_DEBUG_) || defined(_TEST_)
Serial.begin(115200);
while(!Serial);
#endif
runner.startNow();
}
void loop () {
runner.execute();
}