disconnected magcal from core and put it all into hmc5883 driver. no need to keep track of it if driver does init by itself as well.

moved annexcode into mw.c instead of imu.c
hopefully didn't break anything.
NOT FLIGHT TESTED.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@405 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61

src/drv_hmc5883l.c

@@ -74,6 +74,7 @@
 #define HMC_NEG_BIAS 2
 
 static sensor_align_e magAlign = CW180_DEG;
+static float magGain[3] = { 1.0f, 1.0f, 1.0f };
 
 bool hmc5883lDetect(sensor_align_e align)
 {
@@ -90,10 +91,9 @@ bool hmc5883lDetect(sensor_align_e align)
     return true;
 }
 
-void hmc5883lInit(float *calibrationGain)
+void hmc5883lInit(void)
 {
     gpio_config_t gpio;
-    float magGain[3];
     int16_t magADC[3];
     int i;
     int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
@@ -125,12 +125,12 @@ void hmc5883lInit(float *calibrationGain)
         hmc5883lRead(magADC);       // Get the raw values in case the scales have already been changed.
 
         // Since the measurements are noisy, they should be averaged rather than taking the max.
-        xyz_total[0] += magADC[0];
+        xyz_total[X] += magADC[X];
-        xyz_total[1] += magADC[1];
+        xyz_total[Y] += magADC[Y];
-        xyz_total[2] += magADC[2];
+        xyz_total[Z] += magADC[Z];
 
         // Detect saturation.
-        if (-4096 >= min(magADC[0], min(magADC[1], magADC[2]))) {
+        if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
             bret = false;
             break;              // Breaks out of the for loop.  No sense in continuing if we saturated.
         }
@@ -145,21 +145,21 @@ void hmc5883lInit(float *calibrationGain)
         hmc5883lRead(magADC);               // Get the raw values in case the scales have already been changed.
 
         // Since the measurements are noisy, they should be averaged.
-        xyz_total[0] -= magADC[0];
+        xyz_total[X] -= magADC[X];
-        xyz_total[1] -= magADC[1];
+        xyz_total[Y] -= magADC[Y];
-        xyz_total[2] -= magADC[2];
+        xyz_total[Z] -= magADC[Z];
 
         // Detect saturation.
-        if (-4096 >= min(magADC[0], min(magADC[1], magADC[2]))) {
+        if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
             bret = false;
             break;              // Breaks out of the for loop.  No sense in continuing if we saturated.
         }
         LED1_TOGGLE;
     }
 
-    magGain[0] = fabs(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[0]);
+    magGain[X] = fabs(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
-    magGain[1] = fabs(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[1]);
+    magGain[Y] = fabs(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
-    magGain[2] = fabs(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[2]);
+    magGain[Z] = fabs(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);
 
     // leave test mode
     i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x70);   // Configuration Register A  -- 0 11 100 00  num samples: 8 ; output rate: 15Hz ; normal measurement mode
@@ -168,16 +168,9 @@ void hmc5883lInit(float *calibrationGain)
     delay(100);
 
     if (!bret) {                // Something went wrong so get a best guess
-        magGain[0] = 1.0f;
+        magGain[X] = 1.0f;
-        magGain[1] = 1.0f;
+        magGain[Y] = 1.0f;
-        magGain[2] = 1.0f;
+        magGain[Z] = 1.0f;
-    }
-
-    // if parameter was passed, give calibration values back
-    if (calibrationGain) {
-        calibrationGain[0] = magGain[0];
-        calibrationGain[1] = magGain[1];
-        calibrationGain[2] = magGain[2];
     }
 }
 
@@ -187,9 +180,10 @@ void hmc5883lRead(int16_t *magData)
     int16_t mag[3];
 
     i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
-    mag[X] = buf[0] << 8 | buf[1];
+    // During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
-    mag[Z] = buf[2] << 8 | buf[3];
+    // After calibration is done, magGain is set to calculated gain values.
-    mag[Y] = buf[4] << 8 | buf[5];
+    mag[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
-
+    mag[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
+    mag[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
     alignSensors(mag, magData, magAlign);
 }

src/drv_hmc5883l.h

@@ -1,5 +1,5 @@
 #pragma once
 
 bool hmc5883lDetect(sensor_align_e align);
-void hmc5883lInit(float *calibrationGain);
+void hmc5883lInit(void);
 void hmc5883lRead(int16_t *magData);

src/imu.c

@@ -56,7 +56,6 @@ void computeIMU(void)
         accADC[Y] = 0;
         accADC[Z] = 0;
     }
-    annexCode();
 
     if (feature(FEATURE_GYRO_SMOOTHING)) {
         static uint8_t Smoothing[3] = { 0, 0, 0 };

src/mw.c

@@ -84,7 +84,6 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
 
 #define BREAKPOINT 1500
 
-// this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds
 void annexCode(void)
 {
     static uint32_t calibratedAccTime;
@@ -147,7 +146,7 @@ void annexCode(void)
     tmp2 = tmp / 100;
     rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;    // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
 
-    if(f.HEADFREE_MODE) {
+    if (f.HEADFREE_MODE) {
         float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f;
         float cosDiff = cosf(radDiff);
         float sinDiff = sinf(radDiff);
@@ -777,6 +776,7 @@ void loop(void)
         loopTime = currentTime + mcfg.looptime;
 
         computeIMU();
+        annexCode();
         // Measure loop rate just afer reading the sensors
         currentTime = micros();
         cycleTime = (int32_t)(currentTime - previousTime);

src/sensors.c

@@ -366,14 +366,13 @@ void Gyro_getADC(void)
 }
 
 #ifdef MAG
-static float magCal[3] = { 1.0f, 1.0f, 1.0f };     // gain for each axis, populated at sensor init
 static uint8_t magInit = 0;
 
 void Mag_init(void)
 {
     // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
     LED1_ON;
-    hmc5883lInit(magCal);
+    hmc5883lInit();
     LED1_OFF;
     magInit = 1;
 }
@@ -392,10 +391,6 @@ int Mag_getADC(void)
     // Read mag sensor
     hmc5883lRead(magADC);
 
-    magADC[X] = magADC[X] * magCal[X];
-    magADC[Y] = magADC[Y] * magCal[Y];
-    magADC[Z] = magADC[Z] * magCal[Z];
-
     if (f.CALIBRATE_MAG) {
         tCal = t;
         for (axis = 0; axis < 3; axis++) {