fixed mag gain during testmode (thanks pm1) and added some explicit float definitions to mag test code.

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

src/drv_hmc5883l.c

@@ -2,6 +2,62 @@
 
 // HMC5883L, default address 0x1E
 // PB12 connected to MAG_DRDY on rev4 hardware
+// PC14 connected to MAG_DRDY on rev5 hardware
+
+/* CTRL_REGA: Control Register A
+ * Read Write
+ * Default value: 0x10
+ * 7:5  0   These bits must be cleared for correct operation.
+ * 4:2 DO2-DO0: Data Output Rate Bits
+ *             DO2 |  DO1 |  DO0 |   Minimum Data Output Rate (Hz)
+ *            ------------------------------------------------------
+ *              0  |  0   |  0   |            0.75
+ *              0  |  0   |  1   |            1.5
+ *              0  |  1   |  0   |            3
+ *              0  |  1   |  1   |            7.5
+ *              1  |  0   |  0   |           15 (default)
+ *              1  |  0   |  1   |           30
+ *              1  |  1   |  0   |           75
+ *              1  |  1   |  1   |           Not Used
+ * 1:0 MS1-MS0: Measurement Configuration Bits
+ *             MS1 | MS0 |   MODE
+ *            ------------------------------
+ *              0  |  0   |  Normal
+ *              0  |  1   |  Positive Bias
+ *              1  |  0   |  Negative Bias
+ *              1  |  1   |  Not Used
+ *
+ * CTRL_REGB: Control RegisterB
+ * Read Write
+ * Default value: 0x20
+ * 7:5 GN2-GN0: Gain Configuration Bits.
+ *             GN2 |  GN1 |  GN0 |   Mag Input   | Gain       | Output Range
+ *                 |      |      |  Range[Ga]    | [LSB/mGa]  |
+ *            ------------------------------------------------------
+ *              0  |  0   |  0   |  ±0.88Ga      |   1370     | 0xF800?0x07FF (-2048:2047)
+ *              0  |  0   |  1   |  ±1.3Ga (def) |   1090     | 0xF800?0x07FF (-2048:2047)
+ *              0  |  1   |  0   |  ±1.9Ga       |   820      | 0xF800?0x07FF (-2048:2047)
+ *              0  |  1   |  1   |  ±2.5Ga       |   660      | 0xF800?0x07FF (-2048:2047)
+ *              1  |  0   |  0   |  ±4.0Ga       |   440      | 0xF800?0x07FF (-2048:2047)
+ *              1  |  0   |  1   |  ±4.7Ga       |   390      | 0xF800?0x07FF (-2048:2047)
+ *              1  |  1   |  0   |  ±5.6Ga       |   330      | 0xF800?0x07FF (-2048:2047)
+ *              1  |  1   |  1   |  ±8.1Ga       |   230      | 0xF800?0x07FF (-2048:2047)
+ *                               |Not recommended|
+ *
+ * 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
+ *
+ * _MODE_REG: Mode Register
+ * Read Write
+ * Default value: 0x02
+ * 7:2  0   These bits must be cleared for correct operation.
+ * 1:0 MD1-MD0: Mode Select Bits
+ *             MS1 | MS0 |   MODE
+ *            ------------------------------
+ *              0  |  0   |  Continuous-Conversion Mode.
+ *              0  |  1   |  Single-Conversion Mode
+ *              1  |  0   |  Negative Bias
+ *              1  |  1   |  Sleep Mode
+ */
 
 #define MAG_ADDRESS 0x1E
 #define MAG_DATA_REGISTER 0x03
@@ -9,11 +65,11 @@
 #define HMC58X3_R_CONFA 0
 #define HMC58X3_R_CONFB 1
 #define HMC58X3_R_MODE 2
-#define HMC58X3_X_SELF_TEST_GAUSS (+1.16)       // X axis level when bias current is applied.
+#define HMC58X3_X_SELF_TEST_GAUSS (+1.16f)       // X axis level when bias current is applied.
-#define HMC58X3_Y_SELF_TEST_GAUSS (+1.16)       // Y axis level when bias current is applied.
+#define HMC58X3_Y_SELF_TEST_GAUSS (+1.16f)       // Y axis level when bias current is applied.
-#define HMC58X3_Z_SELF_TEST_GAUSS (+1.08)       // Y axis level when bias current is applied.
+#define HMC58X3_Z_SELF_TEST_GAUSS (+1.08f)       // Z axis level when bias current is applied.
-#define SELF_TEST_LOW_LIMIT  (243.0 / 390.0)    // Low limit when gain is 5.
+#define SELF_TEST_LOW_LIMIT  (243.0f / 390.0f)    // Low limit when gain is 5.
-#define SELF_TEST_HIGH_LIMIT (575.0 / 390.0)    // High limit when gain is 5.
+#define SELF_TEST_HIGH_LIMIT (575.0f / 390.0f)    // High limit when gain is 5.
 #define HMC_POS_BIAS 1
 #define HMC_NEG_BIAS 2
 
@@ -58,7 +114,7 @@ void hmc5883lInit(float *calibrationGain)
     i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS);   // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
     // Note that the  very first measurement after a gain change maintains the same gain as the previous setting.
     // The new gain setting is effective from the second measurement and on.
-    i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 2 << 5); // Set the Gain
+    i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011)
     delay(100);
     hmc5883lRead(magADC);
 
@@ -100,9 +156,9 @@ void hmc5883lInit(float *calibrationGain)
         LED1_TOGGLE;
     }
 
-    magGain[0] = fabs(820.0 * HMC58X3_X_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[0]);
+    magGain[0] = fabs(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[0]);
-    magGain[1] = fabs(820.0 * HMC58X3_Y_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[1]);
+    magGain[1] = fabs(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[1]);
-    magGain[2] = fabs(820.0 * HMC58X3_Z_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[2]);
+    magGain[2] = fabs(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[2]);
 
     // 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
@@ -111,9 +167,9 @@ void hmc5883lInit(float *calibrationGain)
     delay(100);
 
     if (!bret) {                // Something went wrong so get a best guess
-        magGain[0] = 1.0;
+        magGain[0] = 1.0f;
-        magGain[1] = 1.0;
+        magGain[1] = 1.0f;
-        magGain[2] = 1.0;
+        magGain[2] = 1.0f;
     }
 
     // if parameter was passed, give calibration values back