Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +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

2
src/drv_serial.c
View file

@ -3,7 +3,7 @@
void serialPrint(serialPort_t *instance, const char *str)
{
uint8_t ch;
while ((ch = *(str++))) {
while ((ch = *(str++)) != 0) {
serialWrite(instance, ch);
}
}

42
src/imu.c
View file

@ -222,11 +222,31 @@ void accSum_reset(void)
accTimeSum = 0;
}
// baseflight calculation by Luggi09 originates from arducopter
static int16_t calculateHeading(t_fp_vector *vec)
{
int16_t head;
float cosineRoll = cosf(anglerad[ROLL]);
float sineRoll = sinf(anglerad[ROLL]);
float cosinePitch = cosf(anglerad[PITCH]);
float sinePitch = sinf(anglerad[PITCH]);
float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;
float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
head = lrintf(hd);
if (head < 0)
head += 360;
return head;
}
static void getEstimatedAttitude(void)
{
uint32_t axis;
int32_t accMag = 0;
static t_fp_vector EstM;
static t_fp_vector EstN = { .A = { 1000.0f, 0.0f, 0.0f } };
static float accLPF[3];
static uint32_t previousT;
uint32_t currentT = micros();
@ -252,6 +272,8 @@ static void getEstimatedAttitude(void)
rotateV(&EstG.V, deltaGyroAngle);
if (sensors(SENSOR_MAG))
rotateV(&EstM.V, deltaGyroAngle);
else
rotateV(&EstN.V, deltaGyroAngle);
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
@ -277,28 +299,16 @@ static void getEstimatedAttitude(void)
angle[ROLL] = lrintf(anglerad[ROLL] * (1800.0f / M_PI));
angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI));
#ifdef MAG
if (sensors(SENSOR_MAG)) {
// baseflight calculation by Luggi09 originates from arducopter
float cosineRoll = cosf(anglerad[ROLL]);
float sineRoll = sinf(anglerad[ROLL]);
float cosinePitch = cosf(anglerad[PITCH]);
float sinePitch = sinf(anglerad[PITCH]);
float Xh = EstM.A[X] * cosinePitch + EstM.A[Y] * sineRoll * sinePitch + EstM.A[Z] * sinePitch * cosineRoll;
float Yh = EstM.A[Y] * cosineRoll - EstM.A[Z] * sineRoll;
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
heading = lrintf(hd);
if (heading < 0)
heading += 360;
}
#endif
if (sensors(SENSOR_MAG))
heading = calculateHeading(&EstM);
else
heading = calculateHeading(&EstN);
acc_calc(deltaT); // rotate acc vector into earth frame
if (cfg.throttle_angle_correction) {
int cosZ = EstG.V.Z / acc_1G * 100.0f;
throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8;
}
}

6
src/main.c
View file

@ -138,9 +138,9 @@ int main(void)
previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = 400;
calibratingG = 1000;
calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
calibratingA = CALIBRATING_ACC_CYCLES;
calibratingG = CALIBRATING_GYRO_CYCLES;
calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
f.SMALL_ANGLES_25 = 1;
// loopy

4
src/mw.c
View file

@ -536,7 +536,7 @@ void loop(void)
i = 0;
// GYRO calibration
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
calibratingG = 1000;
calibratingG = CALIBRATING_GYRO_CYCLES;
if (feature(FEATURE_GPS))
GPS_reset_home_position();
if (sensors(SENSOR_BARO))
@ -578,7 +578,7 @@ void loop(void)
mwArm();
// Calibrating Acc
else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
calibratingA = 400;
calibratingA = CALIBRATING_ACC_CYCLES;
// Calibrating Mag
else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE)
f.CALIBRATE_MAG = 1;

4
src/mw.h
View file

@ -141,6 +141,10 @@ enum {
ALIGN_MAG = 2
};
#define CALIBRATING_GYRO_CYCLES 1000
#define CALIBRATING_ACC_CYCLES 400
#define CALIBRATING_BARO_CYCLES 200
typedef struct config_t {
uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671
uint8_t P8[PIDITEMS];

19
src/sensors.c
View file

@ -124,7 +124,10 @@ retry:
// calculate magnetic declination
deg = cfg.mag_declination / 100;
min = cfg.mag_declination % 100;
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);
}
}

2
src/serial.c
View file

@ -606,7 +606,7 @@ static void evaluateCommand(void)
break;
case MSP_ACC_CALIBRATION:
if (!f.ARMED)
calibratingA = 400;
calibratingA = CALIBRATING_ACC_CYCLES;
headSerialReply(0);
break;
case MSP_MAG_CALIBRATION: