Usando o DMP interno do MPU6050: FIFO, interrupção, quaternions e yaw/pitch/roll com mais estabilidade
O DMP (Digital Motion Processor) do MPU6050 é um bloco interno que executa processamento de movimento e pode despejar resultados já “fundidos” no FIFO, em vez de você ter que fazer toda a fusão no microcontrolador. Na prática, o caminho mais comum (e que funciona de forma reproduzível) é usar uma implementação de referência amplamente difundida, baseada no “MotionApps v2.0”, que inicializa o DMP, carrega o firmware interno apropriado e passa a ler pacotes do FIFO contendo, entre outras coisas, quaternion e dados derivados. Esse fluxo aparece claramente no exemplo DMP6 e no arquivo MotionApps 2.0, que também documenta a estrutura de pacote típica de 42 bytes no FIFO. (Gist)
Do lado “hardware/registradores”, o que torna o DMP usável é a combinação de FIFO (para armazenar pacotes) e interrupção (para avisar quando um pacote novo chegou), ambos descritos no register map do MPU-6000/6050. (TDK InvenSense)
Abaixo eu vou te mostrar um fluxo bem objetivo e “pé no chão” para usar DMP com código C, sem esconder o que está acontecendo. Eu vou assumir, como na seção anterior, que você tem I²C funcionando e consegue fornecer duas funções básicas de read/write. O DMP em si exige um conjunto de rotinas (carregar firmware, configurar offsets, habilitar FIFO/DMP, decodificar pacote). Em vez de “inventar” isso, o caminho correto é incorporar uma biblioteca que já implementa o MotionApps v2.0 (por exemplo, a família I2Cdevlib/derivados que contém MPU6050_6Axis_MotionApps20.h e o exemplo MPU6050_DMP6). (Gist)
1) O que o DMP entrega e por que o quaternion é melhor que Euler direto
O DMP costuma entregar orientação como quaternion (quatro componentes). Quaternion é uma representação de rotação 3D que evita os problemas clássicos de Euler (como gimbal lock) e é numericamente mais estável para acumular rotações. A conversão para yaw/pitch/roll é feita depois, quando você for exibir ou usar em controle, e é aí que você entende por que o pipeline “DMP → quaternion → ângulos” é tão estável em comparação com integrar gyro manualmente. O exemplo DMP6 usa exatamente esse raciocínio: lê o FIFO, extrai quaternion e converte para yaw/pitch/roll. (Gist)
Um detalhe importante e honesto: sem magnetômetro, o yaw (guinada) não tem referência absoluta ao norte e tende a derivar ao longo do tempo. Mesmo com DMP, isso não vira “mágica”; melhora bastante a suavidade e coerência, mas yaw absoluto continua sendo um ponto fraco de IMU 6 eixos. (esp32.com)
2) Inicialização típica do DMP (o que precisa acontecer)
A rotina de inicialização do DMP (via MotionApps v2.0) faz, conceitualmente, quatro coisas: inicializa o MPU6050, carrega o firmware do DMP, aplica offsets (principalmente de giroscópio) e então habilita o DMP com FIFO. O exemplo DMP6 organiza isso de forma clara e é uma boa referência do fluxo “operacional” (incluindo tratamento de overflow). (Gist)
A seguir, um esqueleto fiel ao fluxo do DMP6, mas adaptado para um estilo “C embarcado” (sem depender do Serial do Arduino). Eu vou usar nomes conceituais iguais aos do exemplo porque isso facilita você comparar linha a linha com o que é amplamente testado no campo.
#include <stdint.h>
#include <stdbool.h>
/*
Você precisa incorporar uma implementação MotionApps v2.0.
Em muitos projetos, isso vem como MPU6050_6Axis_MotionApps20.h/.cpp ou equivalente,
junto com funções como:
- mpu.initialize()
- devStatus = mpu.dmpInitialize()
- mpu.setDMPEnabled(true)
- packetSize = mpu.dmpGetFIFOPacketSize()
- mpu.getFIFOCount(), mpu.getFIFOBytes(...)
- mpu.dmpGetQuaternion(...)
- mpu.dmpGetGravity(...)
- mpu.dmpGetYawPitchRoll(...)
Esse conjunto aparece no exemplo DMP6. :contentReference[oaicite:6]{index=6}
*/
// Tipos usuais (os nomes podem variar conforme a lib)
typedef struct { float w, x, y, z; } Quaternion;
typedef struct { float x, y, z; } VectorFloat;
// Estado do DMP
typedef struct {
bool dmpReady;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64]; // 42 bytes típicos, mas deixe folga
} mpu6050_dmp_state_t;
// Saídas “humanas”
typedef struct {
float yaw_deg;
float pitch_deg;
float roll_deg;
} mpu6050_ypr_t;
// Essas funções abaixo são fornecidas pela lib MotionApps/MPU6050 que você incorporar.
extern void mpu_initialize(void);
extern uint8_t mpu_dmpInitialize(void);
extern void mpu_setDMPEnabled(bool en);
extern uint16_t mpu_dmpGetFIFOPacketSize(void);
extern uint8_t mpu_getIntStatus(void);
extern uint16_t mpu_getFIFOCount(void);
extern void mpu_resetFIFO(void);
extern void mpu_getFIFOBytes(uint8_t *data, uint16_t length);
extern void mpu_dmpGetQuaternion(Quaternion *q, const uint8_t *packet);
extern void mpu_dmpGetGravity(VectorFloat *v, const Quaternion *q);
extern void mpu_dmpGetYawPitchRoll(float ypr_rad[3], const Quaternion *q, const VectorFloat *gravity);
// Opcional mas recomendado: setar offsets calibrados (funções variam por lib)
extern void mpu_setXGyroOffset(int16_t v);
extern void mpu_setYGyroOffset(int16_t v);
extern void mpu_setZGyroOffset(int16_t v);
extern void mpu_setZAccelOffset(int16_t v); // alguns exemplos ajustam também accel Z
static float rad2deg(float r) { return r * 57.29577951308232f; } // 180/pi
bool mpu6050_dmp_setup(mpu6050_dmp_state_t *st)
{
mpu_initialize();
// Inicializa DMP (carrega firmware/config interna)
st->devStatus = mpu_dmpInitialize();
// Se devStatus != 0, algo falhou na inicialização do DMP
if (st->devStatus != 0) {
st->dmpReady = false;
return false;
}
// Offsets: esses valores devem vir de calibração no seu hardware.
// No exemplo DMP6, há placeholders típicos que o usuário ajusta.
// Aqui você deve substituir por valores medidos na sua bancada.
mpu_setXGyroOffset(0);
mpu_setYGyroOffset(0);
mpu_setZGyroOffset(0);
// mpu_setZAccelOffset(0); // se sua lib suportar/necessitar
mpu_setDMPEnabled(true);
st->packetSize = mpu_dmpGetFIFOPacketSize(); // tipicamente 42 bytes no MotionApps v2.0 :contentReference[oaicite:7]{index=7}
st->dmpReady = true;
st->fifoCount = 0;
return true;
}
O ponto mais importante aqui é você não tratar setXGyroOffset() como “número mágico”. O DMP melhora muito, mas offsets ruins ainda conseguem estragar o resultado (principalmente drift e comportamento estranho em repouso). O exemplo DMP6 deixa esses offsets explícitos justamente para você calibrar. (Gist)
3) Leitura do FIFO: como extrair pacote, lidar com overflow e converter para ângulos
No mundo real, o FIFO pode overflow se você não ler na frequência adequada, e isso aparece o tempo todo em fóruns e no próprio ecossistema I2Cdevlib: se você coloca um timer lento demais, o FIFO acumula e estoura. A conduta correta é detectar overflow, dar reset no FIFO e seguir o baile, em vez de ficar lendo dados corrompidos. (i2cdevlib.com)
O loop abaixo faz exatamente isso: espera “pacote completo”, lê o pacote e converte para yaw/pitch/roll.
bool mpu6050_dmp_read_ypr(mpu6050_dmp_state_t *st, mpu6050_ypr_t *out)
{
if (!st->dmpReady) return false;
// INT_STATUS e FIFO estão documentados no register map. :contentReference[oaicite:10]{index=10}
const uint8_t intStatus = mpu_getIntStatus();
st->fifoCount = mpu_getFIFOCount();
// Dois cenários típicos:
// 1) overflow -> resetFIFO
// 2) pacote pronto -> ler exatamente packetSize bytes
// Um padrão comum do DMP6 é tratar overflow quando FIFO_count >= 1024
// (tamanho do FIFO do MPU60x0) ou quando o bit de overflow indica problema,
// dependendo da lib. O exemplo DMP6 também simplifica o fluxo assim. :contentReference[oaicite:11]{index=11}
if (st->fifoCount >= 1024) {
mpu_resetFIFO();
st->fifoCount = 0;
return false;
}
// Se ainda não tem um pacote completo, não lê
if (st->fifoCount < st->packetSize) {
return false;
}
// Se tem mais de um pacote acumulado, você pode descartar até ficar com o último
// para reduzir latência (opcional). Aqui vamos ler um pacote por chamada.
mpu_getFIFOBytes(st->fifoBuffer, st->packetSize);
st->fifoCount -= st->packetSize;
Quaternion q;
VectorFloat gravity;
float ypr[3];
mpu_dmpGetQuaternion(&q, st->fifoBuffer);
mpu_dmpGetGravity(&gravity, &q);
mpu_dmpGetYawPitchRoll(ypr, &q, &gravity);
out->yaw_deg = rad2deg(ypr[0]);
out->pitch_deg = rad2deg(ypr[1]);
out->roll_deg = rad2deg(ypr[2]);
return true;
}
Repara que aqui você não está integrando gyro “na unha”, nem aplicando atan2 no acelerômetro para roll/pitch como antes; você está consumindo a saída já processada do DMP, que tende a ser mais suave e estável, especialmente em movimentos rápidos. A estrutura de pacote típica (42 bytes) e o fluxo FIFO→decodificação estão bem estabelecidos nos exemplos e na implementação MotionApps v2.0 que acompanha esse ecossistema. (GitHub)
4) Como isso conversa com “aceleração do objeto” e movimento
Uma vantagem direta de você ter quaternion (ou pelo menos roll/pitch confiáveis) vindo do DMP é que a separação “gravidade vs aceleração linear” fica mais robusta, porque a orientação fica menos ruidosa. O pipeline conceitual é o mesmo que fizemos na seção anterior: estimar gravidade no referencial do sensor e subtrair do acelerômetro. A diferença é que, com DMP, essa orientação tende a ter menos “tremedeira” e menos erro em dinâmicas rápidas, então a sua aceleração linear fica mais “limpa”. Se você quiser, no próximo passo eu te mostro esse mesmo método usando diretamente quaternion (matematicamente mais correto que apenas roll/pitch), para você remover gravidade de forma 3D completa.
