It seems that the wrong MAG calibration did affect the level mode. I am not totally sure about, but as example,
If I pitch my quad to the front:
- calibrated: There will be no Yaw at all shown in GUI.
-wrong calibrated: The Gui will show that the quad is also yawed.
I do not know how this exactly manipulates level mode in flight, but it felt quite smoother after calibrating the MAG.
Yes, I can understand, that wrongly calibrated orientated MAG can cause Yaw drifts and probably level drifts due cross axis sensitivity (BTW: is your ACC level in reference to the frame body?
).
But definitely should not affect anything in case it switched off by switch.. So, you not saying, that just uncommenting MAG in the code (and not switching it on) causing drift?
Btw. drift is now not limited to one direction, it drifts a bit to the left sometimes to the right sometimes forwards and so on.
Recently, I have found that 1.8 is really sensitive during Gyro calibration. Even connecting power on carpet and having slight movement causing slight drift. Can I ask you to execute Gyro calibration by sticks before each
flight, to see is it the reason for your drifts. In case it would help, we can easily modify calibration procedure to make it more "smart". Also note, that gyro drifts quite often not visible in the GUI as we dividing
Gyro values by 8 (as I remember).
I tried to comment the lines you posted. I have to say, drift compensates !much! better now. I did not play with the Gyro<->ACC settings until now. Will do that as soon as my new bearings arrive.
The interesting part: If descending around, quad does a really nice job compensating the drift. Within 2 meters of drifting, it seems to go back to perfect level. It really feels save to fly. Even with heavy wind.
To avoid confusions, I would probably try to explain how this thing works. So let’s start with ACC. ACC is nice device which reports acceleration
, the 3axis ACC reports 3d vector pointing to the acceleration direction and Magnitude of this vector (vector length) is equal (at least should be) to the acceleration value. The trick here that it cannot distinct between latent acceleration and gravitation, so than it sitting flat and still, the vector is pointing straight down to the centre of earth (in case of wmc straight up
) and it should report X = 0, Y = 0, Z = 9.8 m/s/s. In case you accelerating up with 2 m/s/s it should report X = 0, Y = 0, Z = 11.8 m/s/s and if you falling dawn X = 0, Y = 0, Z = 0. And if you accelerating left, let say 9.8 m/s/s, the reported vector would point to center of earth any more, it would inclined left by 45 degrees and length of the vector would be 13.86 m/s/s and it should report X = 0, Y = 9.8, Z = 9.8. The another problem with ACC is vibrations, because vibration is usually linear acceleration, the ACC is designed to sense it, so any vibration in any direction would reported as a jitter of acceleration vector and because vibration acceleration could be quite high, comparatively to the gravitation the jitter could be quite high up to 3-5 degrees.
Let's continue with Gyro, the gyro reports angular rates, which means it reports how fast it is rotated. The 3 axis gyro report how fast it is rotated around all 3 axes. The interesting thing, that it is does not matter how it is moved in space, in case it is not rotated during this movement it reports zeros (In perfect world). The gyro pretty much not sensitive to linear vibrations, but... It is quite easy to make it sense vibrations. What you need is just put it on something soft..
preferably asymmetrically (one side softer than other). Using this recipe we can quite successfully convert linear vibrations to rotations. We can use gyro data to get our position in space just by integrating gyro date, but there are couple of problems: 1) it reporting rates relatively to the body, not to the ground. 2) even very small errors in the rates would cause unbounded growth in the integrated quantities. The Gyro has 2 types of errors (drifts). Static drift, it is sitting still but reporting some movement. And dynamic drift: you rotating it left and right with exactly the same speed but for left rotation it reports, for example, 10 and for right 15..
To overcome those problems, we are doing the following: we integrating Gyro (accumulating vector rotations):
Code: Select all
// Rotate Estimated vector(s), ROLL
deltaGyroAngle = gyroADC[ROLL] * deltaTime;
EstG.V.Z = scos(deltaGyroAngle) * EstG.V.Z - ssin(deltaGyroAngle) * EstG.V.X;
EstG.V.X = ssin(deltaGyroAngle) * EstG.V.Z + scos(deltaGyroAngle) * EstG.V.X;
...
// Rotate Estimated vector(s), PITCH
deltaGyroAngle = gyroADC[PITCH] * deltaTime;
EstG.V.Y = scos(deltaGyroAngle) * EstG.V.Y + ssin(deltaGyroAngle) * EstG.V.Z;
EstG.V.Z = -ssin(deltaGyroAngle) * EstG.V.Y + scos(deltaGyroAngle) * EstG.V.Z;
...
// Rotate Estimated vector(s), YAW
deltaGyroAngle = gyroADC[YAW] * deltaTime;
EstG.V.X = scos(deltaGyroAngle) * EstG.V.X - ssin(deltaGyroAngle) * EstG.V.Y;
EstG.V.Y = ssin(deltaGyroAngle) * EstG.V.X + scos(deltaGyroAngle) * EstG.V.Y;
...
If you comment this part and only ACC would be used to calculate position.
And we correcting results using ACC data:
Code: Select all
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.4G or <0.6G => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if (!((36 > AccMag) or (AccMag > 196))) {
for (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + ACC_VALUE) * INV_GYR_CMPF_FACTOR;
}
If you comment this part and only Gyro would be used to calculate position.
GYR_CMPF_FACTOR – how “strong” correction from ACC.
Now, why we have this part:
Code: Select all
if (!((36 > AccMag) or (AccMag > 196))) {
As we discussed before, in case of latent acceleration the acceleration vector can be shifted from vertical. We are not able to remove latent acceleration from ACC data, so we just limiting total acceleration to 0.6G - 1.4G. With in this range we trust ACC, outside we using only Gyro.
Regards,
Ziss_dm
As you mentioned i just did the flat circle. 
I will give this an try. By the way, in Acro Mode there is no drift at all! Acromode is based on the gyro only? 
I don´t get it. Any ideas are welcome. Feel free to ask for more infos if needed 