Currently we have pretty simple ACC calibration in MultiWii - it good caibrate Z-axis, but zero on X,Y axis strongly depend on orientation of sensors in the moment of calibration.
It working good for hover but not for fast flight: after long time of flight gyro vector follows wrong acc vector, and just after stopping flight leveling is not good.
But for AltHold mode, it is much more important to get uniform ACC vector for full range of inclanation angle. Even small error (2-3 units) will give 1m of altitude error.
The ACC data may be not uniform due to wrong zero calibration and different sensivity for axis.
My idea is pretty simple: measure 6 limits for 3 axis (negative and positive for each) and than calculate Zero and Scale.
My implementation replaces current procedure, but compatible with it: if we calibrate only Z axis in step 1, old method is used.
Code: Select all
void ACC_Common() {
static int32_t a[3];
static uint8_t curAxis = YAW;
int8_t curLimit, axis;
static int16_t limits[3][2] = { {0,0}, {0,0}, {0,0} };
if (calibratingA>0) {
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (calibratingA == 400) {
// Find wich axis to calibrate?
if(abs(accADC[axis]) > abs(accADC[(axis+1)%3]) + abs(accADC[(axis+2)%3])) {
curAxis = axis;
accScale[curAxis] = 0; // re-calibrate scale, too
}
a[axis]=0;
}
// Sum up 400 readings
a[axis] +=accADC[axis];
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (calibratingA == 1) {
curLimit = a[curAxis] > 0 ? 0 : 1; // positive - 0, negative = 1
limits[curAxis][curLimit] = a[curAxis]/400;
// If we get 2 limits for one axis, we can found precize zero and scale
if(limits[curAxis][0] > 0 && limits[curAxis][1] < 0) {
accZero[curAxis] = (limits[curAxis][0] + limits[curAxis][1])/2;
accScale[curAxis] = ((int32_t)acc_1G) * 2000 / (limits[curAxis][0] - limits[curAxis][1]);
}
// Old (not precize) calibration for YAW only
else if(curAxis == YAW && curLimit == 0) {
accZero[YAW] = a[YAW]/400 - acc_1G;
accZero[ROLL] = a[ROLL]/400;
accZero[PITCH] = a[PITCH]/400;
}
accTrim[ROLL] = 0;
accTrim[PITCH] = 0;
writeParams(); // write accZero in EEPROM
}
calibratingA--;
}
// Calibrate
for(axis=0;axis<3;axis++) {
if(accScale[axis]) {
accADC[axis] = ((int32_t)(accADC[axis] - accZero[axis])) * accScale[axis] / 1000;
} else {
accADC[axis]-= accZero[axis];
}
}
}
Also, we need to store AccScale to EEPROM (or recalibrate each time before flight)
How to use:
1. Set quad to ground and press CALIB_ACC button in GUI (or send command from RX by standard stick combination)
2. Turn upside/down (level it as presise as possible) and run calibration again
3. Rotate 90 degree (for instance, PITCH direction up) and run calibration
4. Rotate 180 degree (PITCH directed down) and run calibration
5,6. the same for ROLL.
To re-calibrate by old method, just run step 1 and skip other, and all precise data will be erased.
After this calibration, I get 1G=255 for every angle for every axis, and my AltHold working much better now!