hi
i am implementing tilt compensated compass using the on board sensor
i have calibrated the magnometer and gyros
the angles reading are OK, but the tilt compensation acts funny:
in 0 and 180 degrees the pitch is compensated but the roll isn't
in 90 and 270 degrees the roll is compensated but the pitch isn't
in between these angel you have partial compensation in both pitch and roll
any help is most appreciate
here is the code :
#include "Arduino_BMI270_BMM150.h"
// Variables to hold accelerometer and gyroscope data
float accX, accY, accZ; // Accelerometer readings
float gyroX, gyroY, gyroZ; // Gyroscope readings
float magX, magY, magZ; // Magnetometer readings
float heading; // Compass heading in degrees
float heading_filter=0; // filtered heading
float alfa=0.05; //filter constant
float tiltCompensatedX, tiltCompensatedY; // Tilt compensated values
float pitch, roll; // Pitch and roll angles
unsigned long lastTime; // For timing
float dt; // Time difference
float f = 0.3;
float a = -100;
float b = 360;
void setup() {
Serial.begin(9600);
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU!");
while (1)
;
}
Serial.println("Accelerometer and Gyroscope Initialized");
lastTime = millis();
}
// && IMU.magneticFieldAvailable()
void loop() {
if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable()) {
// Read accelerometer data
IMU.readAcceleration(accX, accY, accZ);
// Read gyroscope data
IMU.readGyroscope(gyroX, gyroY, gyroZ);
// Read manetic data
IMU.readMagneticField(magX, magY, magZ);
// calibrate magnumater and axis aligment
magX=+(magX-9);
magY=-(magY+2);
magZ=-(magZ-10.5);
// Calculate dt
unsigned long currentTime = millis();
dt = (currentTime - lastTime) / 1000.0; // Convert to seconds
lastTime = currentTime;
// Calculate pitch and roll using complementary filter
float accelPitch = atan2(accY, sqrt(accX * accX + accZ * accZ))* 180 / PI ;
float accelRoll = atan2(-accX, accZ)* 180 / PI ;
// Use a complementary filter to combine accelerometer and gyroscope data
pitch = (1 - f) * (pitch + gyroX * dt) + f * accelPitch;
roll = (1 - f) * (roll + gyroY * dt) + f * accelRoll;
pitch=pitch/180*PI;
roll=roll/180*PI;
//pitch=0;
//roll=0;
// Tilt compensation
tiltCompensatedX = magX * cos(pitch) + magZ * sin(pitch);
tiltCompensatedY = magX * sin(roll) * sin(pitch) + magY * cos(roll) - magZ * sin(roll) * cos(pitch);
// Calculate the heading
heading = atan2(tiltCompensatedY, tiltCompensatedX) * 180 / PI;
if (heading < 0) {
heading += 360;
}
heading_filter= (1-alfa)*heading_filter+alfa*heading;
// Normalize heading to 0-360 degrees
if (heading_filter < 0) {
heading_filter += 360;
}
// Print pitch and roll angles
Serial.print(a);
Serial.print("\t");
// Serial.print(pitch);
// Serial.print("\t");
// Serial.print(roll);
// Serial.print("\t");
Serial.print(heading);
Serial.print("\t");
// Serial.print(heading_filter);
// Serial.print("\t");
Serial.print(magX);
Serial.print("\t");
Serial.print(magY);
Serial.print("\t");
Serial.print(magZ);
Serial.print("\t");
Serial.println(b);
delay(10); // Update rate
}
}