Deriva con algoritmo madjwick e mahony

Salve a tutti,
ho un problema di deriva sia per l’algoritmo madjwick che mahony, ho provato ad utilizzare la funzione update invece di updateIMU per inserire anche i dati riguardanti la bussola (ho un mpu9250) ma nonostante ciò e nonostante calibri la bussola, continuo ad avere problemi relativi alla deriva.

Voglio però parlare del contesto in cui lo uso, sto realizzando un self balancing robot utilizzando un esp8266, quando scrivo lo sketch in cui devo leggere e stampare tramite il seriale i valori, dopo l’aggiunta della bussola, i dati sono diventati molto stabili anche se non perfetti, mentre nello sketch di tuning del PID che al momento risulta difficile dato questi problemi, la deriva seppur attenuata continua ad esserci.

In allegato lascio il file .ino dove è presente il codice in cui avvengono questi problemi.

Grazie per l’attenzione :slight_smile:

Self_Balancing_robot_tuning.ino (5.57 KB)

Sto risolvendo utilizzando al posto delle variabili che vengono aggiornate costantemente misurando il tempo, una sorta di timer con interrupt, ovvero la libreria Ticker, o Timer 2 per arduino. Al momento sto riscontrando che calibrando di poco il valore di aggiornamento nel ticker la stabilità ne risente tantissimo.