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

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

src/drv_serial.c

@@ -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);
     }
 }

src/imu.c

@@ -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))
-    if (sensors(SENSOR_MAG)) {
+        heading = calculateHeading(&EstM);
-        // baseflight calculation by Luggi09 originates from arducopter
+    else
-        float cosineRoll = cosf(anglerad[ROLL]);
+        heading = calculateHeading(&EstN);
-        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
 
     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;
-
     }
 }
 

src/main.c

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

src/mw.c

@@ -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;

src/mw.h

@@ -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];

src/sensors.c

@@ -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[ROLL] = (a[ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
-            mcfg.accZero[PITCH] = a[PITCH] / 400;
+            mcfg.accZero[PITCH] = (a[PITCH] + (CALIBRATING_ACC_CYCLES / 2))  / CALIBRATING_ACC_CYCLES;
-            mcfg.accZero[YAW] = a[YAW] / 400 - acc_1G;       // for nunchuk 200=1G
+            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);
             }
         }

src/serial.c

@@ -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: