Hello everyone, I'm developing a project to calibrate, measure and calculate the gyroscope, accelerometer and magnetometer of the Nano 33 BLE sense rev2. I think I calibrated the sensors well, leaving the sensor still for the accelerometer and gyroscope and making "8" in all directions for the magnetometer. When I calculate the pitch, roll and yaw angles, it calculates them correctly except for the yaw. If I calculate yaw only with the gyroscope, a drift appears and it ends up deviating a lot.... I've tried calculating it with the magnetometer and Madgwick filter, but the readings give values over 10000 !! Does anyone know how to calculate yaw in an effective, fast and reliable way? I've read a lot and applied many recommendations, but they have not worked ... I would appreciate any help or advice.
This is my code:
#include <Wire.h>
#include "Arduino_BMI270_BMM150.h"
#include "MadgwickAHRS.h"
//---------------------------------------------------------------------------------------------------------
#define LEDR 22 // Rojo
#define LEDG 23 // Verde
#define LEDB 24 // Azul
#define SensorDeviceName "BioTecBLE_H002" // Nombre del dispositivo BLE asociado a la huella
#define BMI270_I2C_ADDR 0x68 // Dirección I2C del BMI270
#define GYRO_RANGE_REG 0x43 // Dirección del registro para rango del giroscopio
#define GYRO_RANGE_2000 0x00 // ±2000 dps.
#define GYRO_RANGE_1000 0x01 // ±1000 dps.
#define GYRO_RANGE_0500 0x02 // ±500 dps.
#define GYRO_RANGE_0250 0x03 // ±250 dps.
#define GYRO_RANGE_0125 0x04 // ±125 dps.
#define GYRO_CONFIG_REG 0x42 // Dirección del registro para sample rate del giroscopio
#define GYRO_RATE_25Hz 0x06
#define GYRO_RATE_50Hz 0x07
#define GYRO_RATE_100Hz 0x08
#define GYRO_RATE_200Hz 0x09
#define GYRO_RATE_400Hz 0x0a
#define GYRO_RATE_800Hz 0x0b
#define GYRO_RATE_1600Hz 0x0c
#define GYRO_RATE_3200Hz 0x0d
#define ACCEL_RANGE_REG 0x41 // Dirección del registro para rango del acelerometro
#define ACCEL_RANGE_02 0x00 // ±2 g.
#define ACCEL_RANGE_04 0x01 // ±4 g.
#define ACCEL_RANGE_08 0x02 // ±8 g.
#define ACCEL_RANGE_16 0x03 // ±16 g.
#define ACCEL_CONFIG_REG 0x40 // Dirección del registro para sample rate del acelerometro
#define ACCEL_RATE_25Hz 0x06
#define ACCEL_RATE_50Hz 0x07
#define ACCEL_RATE_100Hz 0x08
#define ACCEL_RATE_200Hz 0x09
#define ACCEL_RATE_400Hz 0x0a
#define ACCEL_RATE_800Hz 0x0b
#define ACCEL_RATE_1600Hz 0x0c
#define Intervalo 50 // Intervalo para tomar lecturas analógicas
#define NSAMPLES 1 // Número de muestras del giroscopio
#define NSAMPLES_CALIB 100 // Número de muestras para calibración
#define ALPHA 0.96 // Valor de peso para el cálculo del ángulo
struct TCoord3D{
float x,y,z;
};
//---------------------------------------------------------------------------------------------------------
struct TCalib{
TCoord3D Offset, Max, Min, Range, Scale;
};
TCalib G_Calib, A_Calib, M_Calib;
TCoord3D ang, avgG, avgA, avgM;
float Vmax = -1E6;
float Vmin = 1E6;
unsigned long Tiempo[10];
unsigned long lastTime = 0;
Madgwick filter;
extern TwoWire Wire1;
//---------------------------------------------------------------------------------------------------------
//Rutina para escribir en el registro
void writeRegister(uint8_t reg, uint8_t value) {
Wire1.beginTransmission(BMI270_I2C_ADDR);
Wire1.write(reg); // Dirección del registro
Wire1.write(value); // Valor a escribir
Wire1.endTransmission();
}
//---------------------------------------------------------------------------------------------------------
//Rutina para leer en el registro
uint8_t readRegister(uint8_t reg) {
Wire1.beginTransmission(BMI270_I2C_ADDR);
Wire1.write(reg); // Dirección del registro
Wire1.endTransmission(false);
Wire1.requestFrom(BMI270_I2C_ADDR, 1); // Leer un byte
return Wire1.read();
}
//---------------------------------------------------------------------------------------------------------
uint8_t SetGyroRange(uint8_t GyroRange) {
writeRegister(GYRO_RANGE_REG, GyroRange);
delay(100);
return readRegister(GYRO_RANGE_REG);
}
//---------------------------------------------------------------------------------------------------------
uint8_t SetGyroRate(uint8_t GyroRate) {
writeRegister(GYRO_CONFIG_REG, GyroRate);
delay(100);
return readRegister(GYRO_CONFIG_REG);
}
//---------------------------------------------------------------------------------------------------------
uint8_t SetAccelRange(uint8_t AccelRange) {
writeRegister(ACCEL_RANGE_REG, AccelRange);
delay(100);
return readRegister(ACCEL_RANGE_REG);
}
//---------------------------------------------------------------------------------------------------------
uint8_t SetAccelRate(uint8_t AccelRate) {
writeRegister(ACCEL_CONFIG_REG, AccelRate);
delay(100);
return readRegister(ACCEL_CONFIG_REG);
}
//---------------------------------------------------------------------------------------------------------
void onCharacteristicSubscribed(BLEDevice central, BLECharacteristic characteristic) {
digitalWrite(LEDR, HIGH);
digitalWrite(LEDG, HIGH);
digitalWrite(LEDB, LOW);
delay(50);
digitalWrite(LEDR, HIGH);
digitalWrite(LEDG, LOW);
digitalWrite(LEDB, HIGH);
}
//---------------------------------------------------------------------------------------------------------
void IMU_Calibration(int NS){
int i, NSG, NSA;
String StrXYZ;
avgG.x = avgG.y = avgG.z = 0;
G_Calib.Max.x = G_Calib.Max.y = G_Calib.Max.z = -1E14;
G_Calib.Min.x = G_Calib.Min.y = G_Calib.Min.z = 1E14;
avgA.x = avgA.y = avgA.z = 0;
A_Calib.Max.x = A_Calib.Max.y = A_Calib.Max.z = -1E14;
A_Calib.Min.x = A_Calib.Min.y = A_Calib.Min.z = 1E14;
NSG = NSA = 0;
for (i=0; i < NS; i++){
if (IMU.gyroscopeAvailable()){
digitalWrite(LEDR, LOW);
digitalWrite(LEDG, LOW);
digitalWrite(LEDB, HIGH);
IMU.readGyroscope(G_Calib.Offset.x, G_Calib.Offset.y, G_Calib.Offset.z);
G_Calib.Max.x = max(G_Calib.Offset.x, G_Calib.Max.x);
G_Calib.Max.y = max(G_Calib.Offset.y, G_Calib.Max.y);
G_Calib.Max.z = max(G_Calib.Offset.z, G_Calib.Max.z);
G_Calib.Min.x = min(G_Calib.Offset.x, G_Calib.Min.x);
G_Calib.Min.y = min(G_Calib.Offset.y, G_Calib.Min.y);
G_Calib.Min.z = min(G_Calib.Offset.z, G_Calib.Min.z);
avgG.x += G_Calib.Offset.x;
avgG.y += G_Calib.Offset.y;
avgG.z += G_Calib.Offset.z;
NSG++;
}
if (IMU.accelerationAvailable()){
digitalWrite(LEDR, LOW);
digitalWrite(LEDG, HIGH);
digitalWrite(LEDB, LOW);
IMU.readAcceleration(A_Calib.Offset.x, A_Calib.Offset.y, A_Calib.Offset.z);
A_Calib.Max.x = max(A_Calib.Offset.x, A_Calib.Max.x);
A_Calib.Max.y = max(A_Calib.Offset.y, A_Calib.Max.y);
A_Calib.Max.z = max(A_Calib.Offset.z, A_Calib.Max.z);
A_Calib.Min.x = min(A_Calib.Offset.x, A_Calib.Min.x);
A_Calib.Min.y = min(A_Calib.Offset.y, A_Calib.Min.y);
A_Calib.Min.z = min(A_Calib.Offset.z, A_Calib.Min.z);
avgA.x += A_Calib.Offset.x;
avgA.y += A_Calib.Offset.y;
avgA.z += A_Calib.Offset.z;
NSA++;
}
digitalWrite(LEDR, HIGH);
digitalWrite(LEDG, HIGH);
digitalWrite(LEDB, HIGH);
delay(Intervalo);
}
G_Calib.Range.x = fabs(G_Calib.Max.x - G_Calib.Min.x);
G_Calib.Range.y = fabs(G_Calib.Max.y - G_Calib.Min.y);
G_Calib.Range.z = fabs(G_Calib.Max.z - G_Calib.Min.z);
G_Calib.Offset.x = avgG.x/NSG;
G_Calib.Offset.y = avgG.y/NSG;
G_Calib.Offset.z = avgG.z/NSG;
G_Calib.Scale.x = G_Calib.Range.x / 2.0;
G_Calib.Scale.y = G_Calib.Range.y / 2.0;
G_Calib.Scale.z = G_Calib.Range.z / 2.0;
A_Calib.Range.x = fabs(A_Calib.Max.x - A_Calib.Min.x);
A_Calib.Range.y = fabs(A_Calib.Max.y - A_Calib.Min.y);
A_Calib.Range.z = fabs(A_Calib.Max.z - A_Calib.Min.z);
A_Calib.Offset.x = avgA.x/NSA;
A_Calib.Offset.y = avgA.y/NSA;
A_Calib.Offset.z = avgA.z/NSA;
A_Calib.Scale.x = A_Calib.Range.x / 2.0;
A_Calib.Scale.y = A_Calib.Range.y / 2.0;
A_Calib.Scale.z = A_Calib.Range.z / 2.0;
StrXYZ = String(G_Calib.Max.x, 3) + "; " + String(G_Calib.Max.y, 3) + "; " + String(G_Calib.Max.z, 3) + "; ";
Serial.print("MEDIDAS Max IMU G(x,y,z) : ");
Serial.println(StrXYZ);
StrXYZ = String(G_Calib.Min.x, 3) + "; " + String(G_Calib.Min.y, 3) + "; " + String(G_Calib.Min.z, 3) + "; ";
Serial.print("MEDIDAS Min IMU G(x,y,z) : ");
Serial.println(StrXYZ);
StrXYZ = String(G_Calib.Offset.x, 3) + "; " + String(G_Calib.Offset.y, 3) + "; " + String(G_Calib.Offset.z, 3) + "; ";
Serial.print("MEDIDAS Offset IMU G(x,y,z) : ");
Serial.println(StrXYZ);
StrXYZ = String(G_Calib.Range.x, 3) + "; " + String(G_Calib.Range.y, 3) + "; " + String(G_Calib.Range.z, 3) + "; ";
Serial.print("MEDIDAS Range IMU G(x,y,z) : ");
Serial.println(StrXYZ);
StrXYZ = String(G_Calib.Scale.x, 3) + "; " + String(G_Calib.Scale.y, 3) + "; " + String(G_Calib.Scale.z, 3) + "; ";
Serial.print("MEDIDAS Scale IMU G(x,y,z) : ");
Serial.println(StrXYZ);
Serial.println("-------------------------------------------------------------------------------------------");
StrXYZ = String(A_Calib.Max.x, 3) + "; " + String(A_Calib.Max.y, 3) + "; " + String(A_Calib.Max.z, 3) + "; ";
Serial.print("MEDIDAS Max IMU A(x,y,z) : ");
Serial.println(StrXYZ);
StrXYZ = String(A_Calib.Min.x, 3) + "; " + String(A_Calib.Min.y, 3) + "; " + String(A_Calib.Min.z, 3) + "; ";
Serial.print("MEDIDAS Min IMU A(x,y,z) : ");
Serial.println(StrXYZ);
StrXYZ = String(A_Calib.Offset.x, 3) + "; " + String(A_Calib.Offset.y, 3) + "; " + String(A_Calib.Offset.z, 3) + "; ";
Serial.print("MEDIDAS Offset IMU A(x,y,z) : ");
Serial.println(StrXYZ);
StrXYZ = String(A_Calib.Range.x, 3) + "; " + String(A_Calib.Range.y, 3) + "; " + String(A_Calib.Range.z, 3) + "; ";
Serial.print("MEDIDAS Range IMU A(x,y,z) : ");
Serial.println(StrXYZ);
StrXYZ = String(A_Calib.Scale.x, 3) + "; " + String(A_Calib.Scale.y, 3) + "; " + String(A_Calib.Scale.z, 3) + "; ";
Serial.print("MEDIDAS Scale IMU A(x,y,z) : ");
Serial.println(StrXYZ);
Serial.println("-------------------------------------------------------------------------------------------");
/**/
}
//---------------------------------------------------------------------------------------------------------
void Magnet_Calibration(int NS){
int i, NSM;
float MaxRange, avgEscala;
String StrXYZ;
avgM.x = avgM.y = avgM.z = 0;
M_Calib.Max.x = M_Calib.Max.y = M_Calib.Max.z = -1E14;
M_Calib.Min.x = M_Calib.Min.y = M_Calib.Min.z = 1E14;
NSM = 0;
for (i=0; i < NS;i++){
if (IMU.magneticFieldAvailable()){
digitalWrite(LEDR, HIGH);
digitalWrite(LEDG, LOW);
digitalWrite(LEDB, LOW);
IMU.readMagneticField(M_Calib.Offset.x, M_Calib.Offset.y, M_Calib.Offset.z);
M_Calib.Max.x = max(M_Calib.Offset.x, M_Calib.Max.x);
M_Calib.Max.y = max(M_Calib.Offset.y, M_Calib.Max.y);
M_Calib.Max.z = max(M_Calib.Offset.z, M_Calib.Max.z);
M_Calib.Min.x = min(M_Calib.Offset.x, M_Calib.Min.x);
M_Calib.Min.y = min(M_Calib.Offset.y, M_Calib.Min.y);
M_Calib.Min.z = min(M_Calib.Offset.z, M_Calib.Min.z);
avgM.x += M_Calib.Offset.x;
avgM.y += M_Calib.Offset.y;
avgM.z += M_Calib.Offset.z;
NSM++;
}
digitalWrite(LEDR, HIGH);
digitalWrite(LEDG, HIGH);
digitalWrite(LEDB, HIGH);
delay(Intervalo);
}
M_Calib.Offset.x = avgM.x/NSM;
M_Calib.Offset.y = avgM.y/NSM;
M_Calib.Offset.z = avgM.z/NSM;
M_Calib.Range.x = fabs(M_Calib.Max.x - M_Calib.Min.x);
M_Calib.Range.y = fabs(M_Calib.Max.y - M_Calib.Min.y);
M_Calib.Range.z = fabs(M_Calib.Max.z - M_Calib.Min.z);
MaxRange = M_Calib.Range.x;
MaxRange = max(M_Calib.Range.y, MaxRange);
MaxRange = max(M_Calib.Range.z, MaxRange);
M_Calib.Offset.x = (M_Calib.Max.x + M_Calib.Min.x) / 2.0;
M_Calib.Offset.y = (M_Calib.Max.y + M_Calib.Min.y) / 2.0;
M_Calib.Offset.z = (M_Calib.Max.z + M_Calib.Min.z) / 2.0;
M_Calib.Scale.x = M_Calib.Range.x / 2.0;
M_Calib.Scale.y = M_Calib.Range.y / 2.0;
M_Calib.Scale.z = M_Calib.Range.z / 2.0;
avgEscala = (M_Calib.Scale.x + M_Calib.Scale.y + M_Calib.Scale.z)/3.0;
M_Calib.Scale.x = avgEscala / M_Calib.Scale.x;
M_Calib.Scale.y = avgEscala / M_Calib.Scale.y;
M_Calib.Scale.z = avgEscala / M_Calib.Scale.z;
StrXYZ = String(M_Calib.Max.x, 3) + "; " + String(M_Calib.Max.y, 3) + "; " + String(M_Calib.Max.z, 3) + "; ";
Serial.print("MEDIDAS Max IMU M(x,y,z) : ");
Serial.println(StrXYZ);
StrXYZ = String(M_Calib.Min.x, 3) + "; " + String(M_Calib.Min.y, 3) + "; " + String(M_Calib.Min.z, 3) + "; ";
Serial.print("MEDIDAS Min IMU M(x,y,z) : ");
Serial.println(StrXYZ);
StrXYZ = String(M_Calib.Offset.x, 3) + "; " + String(M_Calib.Offset.y, 3) + "; " + String(M_Calib.Offset.z, 3) + "; ";
Serial.print("MEDIDAS Offset IMU M(x,y,z) : ");
Serial.println(StrXYZ);
StrXYZ = String(M_Calib.Range.x, 3) + "; " + String(M_Calib.Range.y, 3) + "; " + String(M_Calib.Range.z, 3) + "; ";
Serial.print("MEDIDAS Range IMU M(x,y,z) : ");
Serial.println(StrXYZ);
StrXYZ = String(M_Calib.Scale.x, 3) + "; " + String(M_Calib.Scale.y, 3) + "; " + String(M_Calib.Scale.z, 3) + "; ";
Serial.print("MEDIDAS Scale IMU M(x,y,z) : ");
Serial.println(StrXYZ);
Serial.println("-------------------------------------------------------------------------------------------");
}
//---------------------------------------------------------------------------------------------------------
void setup() {
uint8_t GyroNoise, GyroRange, GyroRate, AccelRange, AccelRate;
// Configurar los pines del LED RGB como salidas
pinMode(LEDR, OUTPUT);
pinMode(LEDG, OUTPUT);
pinMode(LEDB, OUTPUT);
/**/
// Inicializamos la comunicación serie
Serial.begin(9600);
while (!Serial);
/**/
// Iniciar IMU
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU!")
while (1);
}
Serial.println("IMU inicializado");
// Inicializamos el acceso al registro
Wire1.begin();
// Configuración frecuencia y rango giroscopio
GyroRange = SetGyroRange(GYRO_RANGE_0250);
GyroRate = SetGyroRate(GYRO_RATE_50Hz);
// Configuración frecuencia y rango acelerometro
AccelRange = SetAccelRange(ACCEL_RANGE_04);
AccelRate = SetAccelRate(ACCEL_RATE_50Hz);
delay(100);
Serial.print("Frecuencia Muestreo Giroscopio = ");
Serial.print(IMU.gyroscopeSampleRate());
Serial.println(" Hz");
Serial.print("Frecuencia Muestreo Acelerómetro = ");
Serial.print(IMU.accelerationSampleRate());
Serial.println(" Hz");
Serial.print("Frecuencia Muestreo Magnetómetro = ");
Serial.print(IMU.magneticFieldSampleRate());
Serial.println(" Hz");
//Calibración IMU
Serial.println("Calibrando Giroscopio y Acelerómetro... 100");
IMU_Calibration(NSAMPLES_CALIB);
Serial.println("Fin Calibración Giroscopio y Acelerómetro... ");
digitalWrite(LEDR, LOW);
digitalWrite(LEDG, LOW);
digitalWrite(LEDB, LOW);
//Calibración Magnetómetro
Serial.println("Calibrando Magnetómetro... ");
Serial.println("Mover el sensor haciendo 8 em todas direcciones");
delay(2000);
Magnet_Calibration(4*NSAMPLES_CALIB);
Serial.println("Fin Calibración Magnetómetro... ");
delay(500);
// Detener BLE si estaba activo
BLE.stopAdvertise();
BLE.stopScan();
BLE.end();
// Opcional: Cambiar la resolución de lectura a 12 bits
analogReadResolution(12); // Cambia la resolución del ADC a 12 bits
Serial.println("Listo para leer (12 bits)");
Serial.println("Esperando conexión con cliente...");
// Encender el LED en azul
digitalWrite(LEDR, HIGH);
digitalWrite(LEDG, HIGH);
digitalWrite(LEDB, LOW);
filter.begin(1000.0/Intervalo);
lastTime = micros(); // Inicializar el tiempo
}
//---------------------------------------------------------------------------------------------------------
void loop() {
TCoord3D g, a, m;
char BufferIMU[512], BufferAAD[512];
String StrXYZ;
int i, analogValue[8];
unsigned long currentTime;
float pitch, roll, yaw, pitchAcc, rollAcc, yawMag, deltaTime, Voltaje, deltat;
roll = pitch = yaw = 0.0;
while (true) {
digitalWrite(LEDR, HIGH);
digitalWrite(LEDG, HIGH);
digitalWrite(LEDB, HIGH);
Tiempo[0] = micros();//inicio lectura AAD;
// Leer el valor analógico de los pins A0 a A7
analogValue[0] = analogRead(A0);
analogValue[1] = analogRead(A1);
analogValue[2] = analogRead(A2);
analogValue[3] = analogRead(A3);
analogValue[4] = analogRead(A4);
analogValue[5] = analogRead(A5);
analogValue[6] = analogRead(A6);
analogValue[7] = analogRead(A7);
Tiempo[1] = micros();//Fin lectura AAD e inicio lectura IMU
if (IMU.gyroscopeAvailable() && IMU.accelerationAvailable() && IMU.magneticFieldAvailable()) {
IMU.readGyroscope(g.x, g.y, g.z);
IMU.readAcceleration(a.x, a.y, a.z);
IMU.readMagneticField(m.x, m.y, m.z);
currentTime = micros();
deltaTime = (currentTime - lastTime); // Convertir a segundos
lastTime = currentTime;
if (fabs(g.x) < fabs(G_Calib.Min.x + G_Calib.Range.x)) avgG.x = 0.0;
if (fabs(g.y) < fabs(G_Calib.Min.y + G_Calib.Range.y)) avgG.y = 0.0;
if (fabs(g.z) < fabs(G_Calib.Min.z + G_Calib.Range.z)) avgG.z = 0.0;
if (fabs(a.x) < fabs(A_Calib.Min.x + A_Calib.Range.x)) avgA.x = 0.0;
if (fabs(a.y) < fabs(A_Calib.Min.y + A_Calib.Range.y)) avgA.y = 0.0;
if (fabs(a.z) < fabs(A_Calib.Min.z + A_Calib.Range.z)) avgA.z = 1.0;
avgG.x = g.x - G_Calib.Offset.x;
avgG.y = g.y - G_Calib.Offset.y;
avgG.z = g.z - G_Calib.Offset.z;
avgA.x = a.x - A_Calib.Offset.x;
avgA.y = a.y - A_Calib.Offset.y;
avgA.z = a.z - A_Calib.Offset.z + 1.0;
avgM.x = (m.x - M_Calib.Offset.x) * M_Calib.Scale.x;
avgM.y = (m.y - M_Calib.Offset.y) * M_Calib.Scale.y;
avgM.z = (m.z - M_Calib.Offset.z) * M_Calib.Scale.z;
avgG.x *= DEG_TO_RAD;
avgG.y *= DEG_TO_RAD;
avgG.z *= DEG_TO_RAD;
filter.update(avgG.x, avgG.y, avgG.z, avgA.x, avgA.y, avgA.z, avgM.x, avgM.y, avgM.z);
// filter.updateIMU(avgG.x, avgG.y, avgG.z, avgA.x, avgA.y, avgA.z);
// Obtener ángulos a partir de lectura giroscopio
roll += avgG.x * 1.0E-6 * deltaTime;
pitch += avgG.y * 1.0E-6 * deltaTime;
yaw += avgG.z * 1.0E-6 * deltaTime;
// Calcular ángulos a partir del acelerómetro (pitchAcc, rollAcc)
pitchAcc = atan2(avgA.x, sqrt(avgA.y * avgA.y + avgA.z * avgA.z)) * RAD_TO_DEG;
rollAcc = atan2(-avgA.y, sqrt(avgA.x * avgA.x + avgA.z * avgA.z)) * RAD_TO_DEG;
yawMag = atan2(avgM.y, avgM.x) * RAD_TO_DEG;
ang.x = filter.getPitch()*RAD_TO_DEG;
ang.y = filter.getRoll()*RAD_TO_DEG;
ang.z = filter.getYaw()*RAD_TO_DEG/360.0;
Tiempo[2] = micros();//Fin lectura IMU
Serial.print("getYaw:");
Serial.print(ang.z);
Serial.print(",yaw:");
Serial.print(yaw);
Serial.print(",yawMag:");
Serial.println(yawMag);
}
digitalWrite(LEDR, HIGH);
digitalWrite(LEDG, LOW);
digitalWrite(LEDB, HIGH);
delay(Intervalo);
}
digitalWrite(LEDR, LOW);
digitalWrite(LEDG, HIGH);
digitalWrite(LEDB, HIGH);
// }
}