added FY90Q buyild target to Makefile

fixed mag calibration finally I think...
heading + mag declination calculation done using better precision.
increased gyro bias calculation to 1000.


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

Makefile

@@ -10,6 +10,8 @@ TCHAIN=
 CC = $(TCHAIN)arm-none-eabi-gcc
 OPT = -Os
 OBJCOPY = $(TCHAIN)arm-none-eabi-objcopy
+# enable for FYxxQ builds
+EXTRA= #-DFY90Q
 
 #Atollic TrueStudio
 #CC = "C:\Program Files (x86)\Atollic\TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0\ARMTools\bin\arm-atollic-eabi-gcc"
@@ -32,7 +34,7 @@ LINK_SCRIPT=stm32_flash.ld
 # Assembler, Compiler and Linker flags and linker script settings
 LINKER_FLAGS=-lm -mthumb -mcpu=cortex-m3 -Wl,--gc-sections -T$(LINK_SCRIPT) -static -Wl,-cref "-Wl,-Map=$(BIN_DIR)/baseflight.map" -Wl,--defsym=malloc_getpagesize_P=0x1000
 ASSEMBLER_FLAGS=-c $(OPT) -mcpu=cortex-m3 -mthumb -x assembler-with-cpp -Isrc -Ilib/STM32F10x_StdPeriph_Driver/inc -Ilib/CMSIS/CM3/CoreSupport -Ilib/CMSIS/CM3/DeviceSupport/ST/STM32F10x
-COMPILER_FLAGS=-c -mcpu=cortex-m3 $(OPT) -Wall -ffunction-sections -fdata-sections -mthumb -DSTM32F10X_MD -DUSE_STDPERIPH_DRIVER -Isrc -Ilib/STM32F10x_StdPeriph_Driver/inc -Ilib/CMSIS/CM3/CoreSupport -Ilib/CMSIS/CM3/DeviceSupport/ST/STM32F10x
+COMPILER_FLAGS=-c -mcpu=cortex-m3 $(OPT) -Wall -ffunction-sections -fdata-sections -mthumb $(EXTRA) -DSTM32F10X_MD -DUSE_STDPERIPH_DRIVER -Isrc -Ilib/STM32F10x_StdPeriph_Driver/inc -Ilib/CMSIS/CM3/CoreSupport -Ilib/CMSIS/CM3/DeviceSupport/ST/STM32F10x
 
 # Define sources and objects
 #

src/drv_hmc5883l.c

@@ -51,7 +51,7 @@ void hmc5883lInit(void)
 void hmc5883lCal(uint8_t calibration_gain)
 {
     // force positiveBias (compass should return 715 for all channels)
-    i2cWrite(MAG_ADDRESS, ConfigRegA, PositiveBiasConfig);
+    i2cWrite(MAG_ADDRESS, ConfigRegA, SampleAveraging_8 << 5 | DataOutputRate_75HZ << 2 | PositiveBiasConfig);
     delay(50);
     // set gains for calibration
     i2cWrite(MAG_ADDRESS, ConfigRegB, calibration_gain);

src/imu.c

@@ -246,8 +246,10 @@ static void getEstimatedAttitude(void)
 #ifdef MAG
     if (sensors(SENSOR_MAG)) {
         // Attitude of the cross product vector GxM
-        heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z) / 10;
+        heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z);
         heading = heading + magneticDeclination;
+        heading = heading / 10;
+
         if (heading > 180)
             heading = heading - 360;
         else if (heading < -180)

src/main.c

@@ -120,7 +120,7 @@ int main(void)
     }
 
     previousTime = micros();
-    calibratingG = 400;
+    calibratingG = 1000;
 
     // loopy
     while (1) {

src/mw.c

@@ -305,7 +305,7 @@ void loop(void)
             rcDelayCommand++;
             if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck && armed == 0) {
                 if (rcDelayCommand == 20) {
-                    calibratingG = 400;
+                    calibratingG = 1000;
                     if (feature(FEATURE_GPS))
                         GPS_reset_home_position();
                 }

src/sensors.c

@@ -104,7 +104,7 @@ retry:
     // calculate magnetic declination
     deg = cfg.mag_declination / 100;
     min = cfg.mag_declination % 100;
-    magneticDeclination = deg + ((float)min * (1.0f / 60.0f));
+    magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
 }
 #endif
 
@@ -278,15 +278,16 @@ static void GYRO_Common(void)
     if (calibratingG > 0) {
         for (axis = 0; axis < 3; axis++) {
             // Reset g[axis] at start of calibration
-            if (calibratingG == 400)
+            if (calibratingG == 1000)
                 g[axis] = 0;
-            // Sum up 400 readings
+            // Sum up 1000 readings
             g[axis] += gyroADC[axis];
+            // g[axis] += (1000 - calibratingG) >> 1;
             // Clear global variables for next reading
             gyroADC[axis] = 0;
             gyroZero[axis] = 0;
             if (calibratingG == 1) {
-                gyroZero[axis] = g[axis] / 400;
+                gyroZero[axis] = g[axis] / 1000;
                 blinkLED(10, 15, 1);
             }
         }
@@ -320,9 +321,10 @@ static void Mag_getRawADC(void)
 
 void Mag_init(void)
 {
+    uint8_t calibration_gain = 0x60; // HMC5883
+#if 1
     uint8_t numAttempts = 0, good_count = 0;
     bool success = false;
-    uint8_t calibration_gain = 0x60; // HMC5883
     uint16_t expected_x = 766;       // default values for HMC5883
     uint16_t expected_yz = 713;
     float gain_multiple = 660.0f / 1090.0f; // adjustment for runtime vs calibration gain
@@ -368,13 +370,28 @@ void Mag_init(void)
         magCal[2] = 1.0f;
     }
 
-#if 0
+    hmc5883lFinishCal();
+#else
+    // initial calibration
+    hmc5883lInit();
+
+    magCal[0] = 0;
+    magCal[1] = 0;
+    magCal[2] = 0;
+
+    // enter calibration mode
+    hmc5883lCal(calibration_gain);
+    delay(100);
+    Mag_getRawADC();
+    delay(10);
+
     magCal[ROLL] = 1160.0f / abs(magADC[ROLL]);
     magCal[PITCH] = 1160.0f / abs(magADC[PITCH]);
     magCal[YAW] = 1080.0f / abs(magADC[YAW]);
-#endif
 
     hmc5883lFinishCal();
+#endif
+
     magInit = 1;
 }
 
@@ -424,7 +441,7 @@ void Mag_getADC(void)
         } else {
             tCal = 0;
             for (axis = 0; axis < 3; axis++)
-                cfg.magZero[axis] = ((magZeroTempMax[axis] - magZeroTempMin[axis]) / 2 - magZeroTempMax[axis]); // Calculate offsets
+                cfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets
             writeParams(1);
         }
     }