Guia de depuração e arquitetura de firmware para MPU6050 + DMP (superloop ou FreeRTOS sem perder pacotes)
Quando o DMP “não funciona”, quase sempre o defeito está em um destes três pontos: I²C instável (pull-up fraco, clock alto demais, ruído), FIFO sendo lido fora de ritmo (overflow ou pacotes “quebrados”), ou calibração/offsets ruins que fazem o resultado parecer “errado” mesmo que o pipeline esteja correto. O segredo é depurar por camadas, sempre validando o que é determinístico antes de culpar o DMP.
A primeira camada é provar que o MPU responde no I²C e que você lê dados coerentes dos registradores “crus”. Mesmo que seu objetivo final seja DMP, ler ACCEL_XOUT_H e ver o módulo do acelerômetro próximo de 1 g em repouso e o giroscópio próximo de 0 °/s já elimina 80% das causas. Se o cru já vem “saltando”, travando ou com valores absurdos, o DMP só vai mascarar o problema por alguns instantes e depois desandar.
A segunda camada é provar que você está lendo o FIFO em pacotes completos e no ritmo certo. A regra de ouro é: você nunca deve “pegar metade do pacote” nem ler com tamanho variável. Você mede fifoCount, garante que fifoCount >= packetSize e então lê exatamente packetSize. Se fifoCount encosta no limite do FIFO, você faz resetFIFO() e recomeça, porque dali para frente os dados são potencialmente corrompidos. Se você está vendo overflow frequentemente, não adianta “tunar filtro”: é taxa de leitura/latência do loop/RTOS.
A terceira camada é validar orientação. Um teste simples e muito revelador é colocar o sensor parado em uma mesa e girar lentamente em um único eixo. Pitch e roll devem ficar estáveis e repetir o mesmo comportamento toda vez. Se o yaw “andar” lentamente mesmo parado, isso pode ser bias ainda presente no gyro (normal) e ausência de referência absoluta (normal também). O que não pode acontecer é pitch/roll pularem ou invertirem aleatoriamente, porque isso costuma apontar para ordem de eixos (montagem), sinal invertido, pacote do FIFO fora de alinhamento ou overflow sendo ignorado.
Superloop robusto com INT: ler FIFO apenas quando chegar dado novo
A forma mais “saudável” de usar DMP é deixar a interrupção do pino INT apenas sinalizar que existe dado novo e ler o FIFO no loop principal. Isso evita fazer I²C dentro de ISR (péssima ideia na maioria das plataformas) e mantém o sistema previsível.
#include <stdbool.h>
#include <stdint.h>
static volatile bool g_mpuIntFlag = false;
// ISR do seu GPIO ligado ao INT do MPU (borda de subida normalmente)
void MPU_INT_ISR(void)
{
g_mpuIntFlag = true;
}
void app_loop(void)
{
mpu6050_dmp_state_t dmp;
mpu6050_ypr_t ypr;
if (!mpu6050_dmp_setup(&dmp)) {
// falhou inicialização do DMP: aqui você cai para modo cru/filtro complementar
for (;;) {}
}
for (;;) {
if (!g_mpuIntFlag) {
// aqui você pode fazer outras tarefas leves
continue;
}
g_mpuIntFlag = false;
// lê um pacote quando houver
if (mpu6050_dmp_read_ypr(&dmp, &ypr)) {
// use ypr.yaw_deg / pitch_deg / roll_deg
// envie por UART, atualize controle, etc.
}
}
}
O ponto de engenharia aqui é que a interrupção não é usada para “processar”, e sim para acordar o loop. Isso reduz jitter e reduz chances de overflow, porque o sistema passa a reagir “no evento”, não “quando der”.
Em FreeRTOS: ISR dá um “sinal”, tarefa lê FIFO e processa
Em RTOS, a arquitetura boa é a mesma: ISR só sinaliza, a tarefa do MPU lê o FIFO e atualiza uma estrutura de estado compartilhada. Você evita fazer I²C dentro da interrupção e também evita que outras tarefas (Wi-Fi, BLE, display) atrapalhem a cadência do FIFO.
Abaixo vai um esqueleto típico usando notificação direta de tarefa (é leve e rápido). Ajuste para o seu RTOS/porta.
// Pseudocódigo FreeRTOS (depende do seu port, mas a lógica é esta)
#include "FreeRTOS.h"
#include "task.h"
static TaskHandle_t g_mpuTaskHandle = NULL;
void MPU_INT_ISR(void)
{
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
vTaskNotifyGiveFromISR(g_mpuTaskHandle, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
void vMpuTask(void *arg)
{
mpu6050_dmp_state_t dmp;
mpu6050_ypr_t ypr;
if (!mpu6050_dmp_setup(&dmp)) {
vTaskDelete(NULL);
}
for (;;) {
// espera interrupção do MPU
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
// pode haver mais de um pacote acumulado; ler 1 por vez ou drenar
while (mpu6050_dmp_read_ypr(&dmp, &ypr)) {
// publique em uma fila, buffer atômico, ou proteja com mutex
// exemplo: xQueueOverwrite(queueYPR, &ypr);
}
}
}
O “pulo do gato” aqui é o while: se o sistema ficou ocupado por alguns milissegundos, o FIFO pode ter acumulado mais de um pacote. Drenar evita que você fique sempre atrasado. Se seu objetivo é menor latência e você não precisa de todos os pacotes, você pode descartar até ficar com o último (isso é uma decisão de controle, não de biblioteca).
Como identificar rápido a causa quando “está ruim”
Se seus ângulos parecem “tremidos” em repouso, normalmente é ruído mecânico/vibração, DLPF inadequado, alimentação ruidosa ou módulo ruim. Se os ângulos pulam como se tivessem “teleport”, costuma ser FIFO fora de alinhamento (ler tamanho errado, overflow não tratado, ou leitura sem respeitar packetSize). Se tudo parece certo por alguns segundos e depois degrada, quase sempre é overflow por falta de leitura ou dt/cadência irregular no seu loop.
Se o pitch e roll parecem invertidos ou trocados, isso quase sempre é montagem/eixos. O MPU mede no referencial do chip, não no “do seu robô”; então se você rotacionou a placa 90° na montagem, você precisa remapear eixos no software (ou reinterpretar roll/pitch conforme sua convenção). Isso não é “calibração”, é geometria do sistema.
Fechando o ciclo com o que você pediu: por que o DMP melhora “resultados”
O DMP melhora principalmente em dois pontos práticos: estabilidade de orientação em movimentos rápidos (porque ele faz fusão interna e entrega quaternion) e redução de carga no MCU (porque você lê pacotes prontos do FIFO). Isso não elimina física do sensor: yaw absoluto continua sem referência externa, e “posição por IMU” continua derivando se você integrar sem correções. Mas para o que a maioria dos projetos com MPU6050 realmente quer (inclinação confiável, orientação suave, aceleração linear melhor separada), o caminho DMP + FIFO + quaternion costuma ser mais consistente do que integrar gyro e filtrar “na mão”.
Referências
As informações técnicas apresentadas neste artigo foram obtidas diretamente da documentação oficial do fabricante do MPU6050 (MPU-6000/MPU-6050) e de implementações de referência amplamente utilizadas na comunidade técnica para uso do DMP (Digital Motion Processor), incluindo exemplos que documentam o fluxo completo de inicialização, leitura do FIFO e extração de quaternion e yaw/pitch/roll.
Documentação oficial do dispositivo (mapa de registradores, configuração de escalas, DLPF, FIFO, interrupções e estrutura de dados):
- TDK InvenSense. MPU-6000 and MPU-6050 Register Map and Descriptions.
Disponível em:
https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf - TDK InvenSense. MPU-6000/MPU-6050 Product Specification.
(Especificação elétrica, escalas do acelerômetro e giroscópio, características do sensor.)
Disponível em:
https://invensense.tdk.com/products/motion-tracking/6-axis/mpu-6050/
Implementação de referência para uso do DMP (MotionApps v2.0, estrutura de pacote de 42 bytes, extração de quaternion, gravity e yaw/pitch/roll):
- Jeff Rowberg – I2Cdevlib – MPU6050 DMP6 Example e MPU6050_6Axis_MotionApps20.
(Fluxo prático de inicialização do DMP, habilitação, leitura de FIFO e conversão para YPR.)
Exemplo DMP6 (espelho público):
https://gist.github.com/dbef638aa988f03433d6Biblioteca I2Cdevlib (repositório):
https://github.com/jrowberg/i2cdevlib
Discussões técnicas relevantes sobre uso prático e problemas comuns (overflow de FIFO, estabilidade, integração com MCU):
- I2Cdevlib Forum – Discussões sobre FIFO overflow e uso do DMP:
https://www.i2cdevlib.com/forums/ - Comunidade técnica ESP32 – Discussões sobre MPU6050 e DMP:
https://esp32.com/
As fórmulas trigonométricas para cálculo de roll e pitch a partir do vetor gravidade e as expressões para obtenção do vetor gravidade a partir do quaternion são derivadas diretamente da matemática de rotação tridimensional (cinemática de corpos rígidos e álgebra de quaternions), aplicadas conforme o modelo físico do acelerômetro e do giroscópio descrito na especificação do dispositivo.
