Salve a tutti, sto usando una MPU6050 per compensare l'inclinazione di un magnetometro QMC5883l, attualmente funziona bene quando lo inclino sull'asse Y, ma sull'asse X ha delle piccole variazioni che vanno da 1/9 gradi in base all'angolo. Il codice che uso:
#include <Wire.h>
#include <QMC5883L.h>
QMC5883L compass;
//Gyro x,y,z raw values
int gyro_x;
int gyro_y;
int gyro_z;
//Acc x,y,z raw values and total acceleration
long acc_x;
long acc_y;
long acc_z;
long acc_total_vector;
//IMU temperature
int temperature;
//Gyro calibration values
long gyro_x_cal;
long gyro_y_cal;
long gyro_z_cal;
//Calculated angles on x,y,z
float angle_pitch;
float angle_roll;
float angle_yaw;
//"are gyro angle already set?" value
boolean set_gyro_angles;
//x,y acc angles
float angle_roll_acc;
float angle_pitch_acc;
//x,y output angles
float angle_pitch_output;
float angle_roll_output;
unsigned long loop_timer = 0;
int16_t xhigh, xlow;
int16_t yhigh, ylow;
int16_t zhigh, zlow;
void setup()
{
Wire.begin();
TWBR = 12;
Serial.begin(57600);
compass.init();
compass.setSamplingRate(50);
IMU_setup();
Serial.println("Turn compass in all directions to calibrate....");
loop_timer = micros();
}
void loop()
{
//give a 250Hz loop frequency
while(micros() - loop_timer < 4000); //wait until 4000us are passed.
loop_timer = micros();
IMU_calc();
MAGN_calc();
}
void MAGN_calc(){
if(compass.ready()){
int cheading = readTiltCompensatedHeading();
if(cheading == 0){
}
else{
Serial.print(" ");
Serial.println(cheading);
}
}
}
int readTiltCompensatedHeading()
{
int16_t x, y, z, t;
if(!compass.readRaw(&x,&y,&z,&t)) return 0;
/* Update the observed boundaries of the measurements */
if(x<xlow) xlow = x;
if(x>xhigh) xhigh = x;
if(y<ylow) ylow = y;
if(y>yhigh) yhigh = y;
if(z<zlow) zlow = z;
if(z>zhigh) zhigh = z;
/* Bail out if not enough data is available. */
if( xlow==xhigh || ylow==yhigh || zlow==zhigh) return 0;
/* Recenter the measurement by subtracting the average */
x -= (xhigh+xlow)/2;
y -= (yhigh+ylow)/2;
z -= (zhigh+zlow)/2;
/* Rescale the measurement to the range observed. */
float fx = (float)x/(xhigh-xlow);
float fy = (float)y/(yhigh-ylow);
float fz = (float)z/(zhigh-zlow);
//#### VERIFY
float Xh = (float)x * cos(angle_roll_output * 0.0174533) + (float)z * sin(angle_roll_output * 0.0174533);
float Yh = (float)x * sin(angle_pitch_output * 0.0174533) * sin(angle_roll_output * 0.0174533) + (float)y * cos(angle_pitch_output * 0.0174533) - (float)z * sin(angle_pitch_output * 0.0174533) * cos(angle_roll_output * 0.0174533);
//#### VERIFY
int heading = atan2(Yh,Xh)*180/PI;
if(heading<=0) heading += 360;
return heading;
}
void IMU_setup(){
setup_mpu_6050_registers(); //Setup the registers of the MPU-6050 (500dfs / +/-8g) and start the gyro
Serial.println("Calibrating gyro");
for (int cal_int = 0; cal_int < 2000 ; cal_int ++){ //Run this code 2000 times
read_mpu_6050_data(); //Read the raw acc and gyro data from the MPU-6050
gyro_x_cal += gyro_x; //Add the gyro x-axis offset to the gyro_x_cal variable
gyro_y_cal += gyro_y; //Add the gyro y-axis offset to the gyro_y_cal variable
gyro_z_cal += gyro_z; //Add the gyro z-axis offset to the gyro_z_cal variable
delay(3); //Delay 3ms to simulate the 250Hz program loop
}
gyro_x_cal /= 2000; //Divide the gyro_x_cal variable by 2000 to get the avarage offset
gyro_y_cal /= 2000; //Divide the gyro_y_cal variable by 2000 to get the avarage offset
gyro_z_cal /= 2000; //Divide the gyro_z_cal variable by 2000 to get the avarage offset
}
void IMU_calc(){
read_mpu_6050_data(); //Read the raw acc and gyro data from the MPU-6050
gyro_x -= gyro_x_cal; //Subtract the offset calibration value from the raw gyro_x value
gyro_y -= gyro_y_cal; //Subtract the offset calibration value from the raw gyro_y value
gyro_z -= gyro_z_cal; //Subtract the offset calibration value from the raw gyro_z value
//Gyro angle calculations
//0.0000611 = 1 / (250Hz / 65.5)
angle_pitch += gyro_x * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_roll += gyro_y * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable
angle_yaw = gyro_z / 65.5;
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel
//Accelerometer angle calculations
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle
//Place the MPU-6050 spirit level and note the values in the following two lines for calibration
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch
angle_roll_acc -= 0.0; //Accelerometer calibration value for roll
if(set_gyro_angles){ //If the IMU is already started
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle
}
else{ //At first start
angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle
angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle
set_gyro_angles = true; //Set the IMU started flag
}
//To dampen the pitch and roll angles a complementary filter is used
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value
Serial.print(angle_pitch_output);
Serial.print(" ");
Serial.println(angle_roll_output);
}
Qualche idea sul motivo per cui sull'asse X non funzioni bene?