Stamattina ho preso carta e penna e ho cominciato a scrivere qualche calcolo e ne è risultato fuori questo programma:
#include <Wire.h>
#include <ITG3200.h>
#define address 0x40
ITG3200 gyro = ITG3200();
float gyroX, gyroY, gyroZ;
float accX, accY, accZ;
float angoloAssoluto=0;
long previousMicros = 0;
unsigned long currentMicros;
void setup(void) {
Serial.begin(9600);
Wire.begin();
initBMA180();
delay(1000);
gyro.init(ITG3200_ADDR_AD0_LOW);
Serial.print("zeroCalibrating...");
gyro.zeroCalibrate(2500, 2);
Serial.println("done.");
previousMicros = micros();
}
void loop(void) {
while (gyro.isRawDataReady()) {
currentMicros = micros();
gyro.readGyro(&gyroX, &gyroY, &gyroZ);
Serial.print(gyroX);Serial.print("\t");
accX = readAccel3(0x03, 0x02);
accY = readAccel3(0x05, 0x04);
accZ = readAccel3(0x07, 0x06);
float intervallo = currentMicros-previousMicros; //trovo l'intervallo trascorso fra una lettura e l'altra
angoloAssoluto+=gyroX*(intervallo/1000000); //moltiplico l'accelerazione angolare (°/sec) per il tempo in secondi (dividendo quindi il valore in microsecondi per 1000000). Incremento quindi l'angolo assoluto.
Serial.print(angoloAssoluto);Serial.print("\t");
Serial.print(intervallo);Serial.print("\n");
previousMicros = currentMicros;
}
}
effettivamente ruotando di 90° il socket su cui c'è montata l'imu sul monitor seriale leggo un valore di circa 90 (che poi molto lentamente si incrementa perchè la velocità angolare non è mai 0). Questa cosa però mi suona un po' strana perchè l'accelerazione il la ottengo in rad/sec, e non effettuo nessuna conversione e come è possibile che il risultato è in gradi?
ho detto una bella cazzata, il valore l'ottengo in gradi al secondo... a questo punto dovrebbe essere corretto, non mi resta che applicare il filtro di kalman...
AGGIORNAMENTO: se quando faccio il calcolo dell'angolo assoluto il valore della velocità angolare lo passo come intero noto che il valore finale non viene incrementato col tempo, mantenendo una certa sensibilità; resta ovviamente il problema del drift....
AGGIORNAMENTO 2: Applicando la soluzione di sopra aumenta l'errore in movimento, quindi conviene lasciare il codice scritto in alto...