need help !

[chourouk]

Hello,



Here there are two weeks that I'm stuck and I need your help, I'm looking how to find YAW using a MPU9150 but I can not, I actually have the compass the accelerometer and gyroscope data's, I managed to calculate the pitch and roll but not the Yaw, however I could figure out the yaw when pitch and roll = 0 (using this formul:

yh = fcmps [1] * cos (rollaccm [k]) - fcmps [2] * sin (rollaccm [k]);
xh fcmps = [0] * cos (pitchaccm [k]) + fcmps [1] * sin (rollaccm [k]) * sin (pitchaccm [k]) + fcmps [2] * cos (rollaccm [k]) * sin (pitchaccm [k]);

yaw = (atan2 (XH, YH) * 180.0) / M_PI; )

my project is about a sensor gloves !
please, i request if it is necessary to calibrate the sensor to get there? (Because I did not calibrate my MPU) if so how i can calibrate it?



Thank you for your answer!

[Plüschi]

Look here: http://www.starlino.com/imu_guide.html

ACC gives you a 3D vector, pointing downwards at rest. This vector has NO yaw data.
For yaw you have to integrate the gyro and fusion it with the mag data.

Mag has another twist, its another 3D vector NOT pointing Nord but depending where you live pointing downwards at an angle. You have to subtract acc vector from mag vector to get N direction.

Theoretically you can use only acc and mag. BUT since acc and mag are chronically noisy you need to average them, which will make the system slow and laggy. By adding gyro to the heavily filtzered acc you get a fast and precise system. The math behind is hard, but results are excellent.

Copy the mwii imu.cpp code, its all there.

[chourouk]

Thank you for replying and showing interest in my topic, indeed i'm beginner and i dont understand this part of the mwii's code :


EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + imu.accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
EstG32.A[axis] = EstG.A[axis]; //int32_t cross calculation is a little bit faster than float
#if MAG
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + imu.magADC[axis]) * INV_GYR_CMPFM_FACTOR;
EstM32.A[axis] = EstM.A[axis];
#endif

why they choose the coefficient GYR_CMPFM_FACTOR = 250 , GYR_CMPF_FACTOR = 600 ?
Are imu.magADC,imu.accSmooth accelerometer's and magnetometer's data ?

Sorry for my english .

[chourouk]

Look here: http://www.starlino.com/imu_guide.html

ACC gives you a 3D vector, pointing downwards at rest. This vector has NO yaw data.
For yaw you have to integrate the gyro and fusion it with the mag data.

Mag has another twist, its another 3D vector NOT pointing Nord but depending where you live pointing downwards at an angle. You have to subtract acc vector from mag vector to get N direction.

Theoretically you can use only acc and mag. BUT since acc and mag are chronically noisy you need to average them, which will make the system slow and laggy. By adding gyro to the heavily filtzered acc you get a fast and precise system. The math behind is hard, but results are excellent.

Copy the mwii imu.cpp code, its all there.

Thank you for replying and showing interest in my topic, indeed i'm beginner and i dont understand this part of the mwii's code :


EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + imu.accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
EstG32.A[axis] = EstG.A[axis]; //int32_t cross calculation is a little bit faster than float
#if MAG
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + imu.magADC[axis]) * INV_GYR_CMPFM_FACTOR;
EstM32.A[axis] = EstM.A[axis];
#endif

why they choose the coefficient GYR_CMPFM_FACTOR = 250 , GYR_CMPF_FACTOR = 600 ?
Are imu.magADC,imu.accSmooth accelerometer's and magnetometer's data ?

Sorry for my english .

[Plüschi]

Please stop opening different threads on the same topic.

EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + imu.accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
translates into
Acc_vector = (Acc_vector * 99.0 + new_accdata) * 0.01;

This is a filter. The new_accdata creeps into Acc_vector only very slowly. Eliminates noisiness and eliminates disturbed accdata, think of loopings.
Input is imu.accSmooth, already smoothed accdata (there is no need for presmooth, no idea why its done). Output is EstG, our 3D gravity vector.

Gyro is fast + precise, but has long term drift.
Acc is noisy and full of disturbances, but has zero drift.
So basically we take gyro data, integrate it, and correct the drift with acc data.

[chourouk]

OK thank you, and have we also need to correct the compass data with the gyro ?
I'm sorry for opening different threads on the same topic. i didn't think that is prohibited, i will remove it.

[Plüschi]

Same stuff for MAG.

BTW
I guess the 650 factor is a compromise. Too high values will compromise the ability to correct gyro drift, while low values will destabilize the level function.

The used filter looks similar to a first order Butterworth LowPass (i compared results).
Assuming a looptime of 2ms, which is 500hz, the factor 650 results in about 0.25hz cutoff, means 4 seconds.
Baseflight with its default 3.5ms looptime would get a 0.15 hz cutoff, or 7 seconds.

A second order Butterworth LowPass with same cutoff looks better, approaches final value faster in a more linear slope. Just an idea for improvement. Code example:
http://www-users.cs.york.ac.uk/~fisher/ ... /trad.html

[chourouk]

but here i can not see where the gyro is intervened, "Acc_vector = (Acc_vector * 99.0 + new_accdata) * 0.01;"
in my project i use also 10 MPU6050 and 1 MPU9150, i request can the 10 mpu6050 disrupt compass's mesurements ?( i see topics talking about same magnetic attraction )
(sorry for my stupid questions but it seems to complicated for me).

[Plüschi]

rotateV(&EstG.V,deltaGyroAngle);

rotate geravity vector by gyro angle. This integrates (sums up) gyro angles. But singe G has no yaw component we do

rotateV(&EstM.V,deltaGyroAngle);

which has a yaw component. We need two 3-D vectors to define all 3 axes.

[chourouk]

Hello,


I show you my code, I almost Copy the mwii imu.cpp code but I dont find the mistake, can you help me please ?

Code: Select all

 float calculate_yaw() 
{ 
    int16_t yawm; 
    
    pitchm = (pitch[10]*M_PI)/180.0;  //( pitchm en rad)
    rollm = (roll[10]*M_PI)/180.0;  //( rollm en rad)

  
    float cosineRoll = cos(rollm); 
    float sineRoll = sin(rollm); 
    float cosinePitch = cos(pitchm); 
    float sinePitch = sin(pitchm); 



//rotateV(&EstM.V,deltaGyroAngle) ==
    fM[0] = M[0];
    fM[1] = M[1];
    fM[2] = M[2]; 

    M[2] -= Yg_tp[10]  * fM[0] + Xg_tp[10] * fM[1];   //   Xg_tp[k] = Xg[k] * dt[k]  deltaGyroAngle
    M[0] += Yg_tp[10]  * fM[2] - Zg_tp[10]   * fM[1];
    M[1] += Xg_tp[10] * fM[2] +  Zg_tp[10]   * fM[0];
   

// EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + imu.magADC[axis]) * INV_GYR_CMPFM_FACTOR==
    M[0] = (M[0] * 0.99 + cmps[0]) * 0.01;
    M[1] = (M[1] * 0.99 + cmps[1]) * 0.01;
    M[2] = (M[2] * 0.99 + cmps[2]) * 0.01;


//heading
    float Xh =  M[0]* cosinePitch +  M[1] * sineRoll * sinePitch + M[2] * sinePitch * cosineRoll; 
    float Yh =  M[1] * cosineRoll - M[2] * sineRoll; 
    yawm = (atan2(Yh, Xh) * 1800.0f / M_PI + mag_declination)/10 ; 

    
    if (yawm < 0) 
       yawm += 360; 
       
       
    return yawm; 
}