Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

calculate heading using gyro-only on boards without mag - idea by Cesco

Browse source 
added constants for gyro/acc/baro cal and fixed calibration to add /2
warning cleanup in drv_serial.c

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@426 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2013-10-02 02:18:14 +00:00
parent 5332b78200
commit 3744f36895
7 changed files with 49 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

21
src/sensors.c
View file

@ -124,7 +124,10 @@ retry:
// calculate magnetic declination
deg = cfg.mag_declination / 100;
min = cfg.mag_declination % 100;
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
if (sensors(SENSOR_MAG))
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
else
magneticDeclination = 0.0f;
}
#endif
@ -165,9 +168,9 @@ static void ACC_Common(void)
if (calibratingA > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (calibratingA == 400)
if (calibratingA == CALIBRATING_ACC_CYCLES)
a[axis] = 0;
// Sum up 400 readings
// Sum up CALIBRATING_ACC_CYCLES readings
a[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
@ -175,9 +178,9 @@ static void ACC_Common(void)
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (calibratingA == 1) {
mcfg.accZero[ROLL] = a[ROLL] / 400;
mcfg.accZero[PITCH] = a[PITCH] / 400;
mcfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G
mcfg.accZero[ROLL] = (a[ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
mcfg.accZero[PITCH] = (a[PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
mcfg.accZero[YAW] = (a[YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
cfg.angleTrim[ROLL] = 0;
cfg.angleTrim[PITCH] = 0;
writeEEPROM(1, true); // write accZero in EEPROM
@ -334,7 +337,7 @@ static void GYRO_Common(void)
if (calibratingG > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (calibratingG == 1000) {
if (calibratingG == CALIBRATING_GYRO_CYCLES) {
g[axis] = 0;
devClear(&var[axis]);
}
@ -348,14 +351,14 @@ static void GYRO_Common(void)
float dev = devStandardDeviation(&var[axis]);
// check deviation and startover if idiot was moving the model
if (mcfg.moron_threshold && dev > mcfg.moron_threshold) {
calibratingG = 1000;
calibratingG = CALIBRATING_GYRO_CYCLES;
devClear(&var[0]);
devClear(&var[1]);
devClear(&var[2]);
g[0] = g[1] = g[2] = 0;
continue;
}
gyroZero[axis] = g[axis] / 1000;
gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
blinkLED(10, 15, 1);
}
}