From cf643b98c80eb14ac6a66d81d9694ceaeab6f469 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Wed, 15 Oct 2014 22:42:07 +0100
Subject: [PATCH] Use Naze hardware revision when initialising hmc5883.  The
 driver is not not naze specific anymore.

---
 src/main/drivers/compass_hmc5883l.c | 22 +++++++++-------------
 src/main/drivers/compass_hmc5883l.h | 10 ++++++++--
 src/main/sensors/compass.c          | 26 +++++++++++++++++++++++++-
 3 files changed, 42 insertions(+), 16 deletions(-)

diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c
index 1ee95f09da..5273c964fb 100755
--- a/src/main/drivers/compass_hmc5883l.c
+++ b/src/main/drivers/compass_hmc5883l.c
@@ -122,29 +122,25 @@ bool hmc5883lDetect(void)
     return true;
 }
 
-void hmc5883lInit(void)
+void hmc5883lInit(hmc5883Config_t *hmc5883Config)
 {
     int16_t magADC[3];
     int i;
     int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
     bool bret = true;           // Error indicator
 
-#ifdef NAZE
     gpio_config_t gpio;
-    if (hse_value == 8000000) {
-        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
-        // PB12 - MAG_DRDY output on rev4 hardware
-        gpio.pin = Pin_12;
+
+    if (hmc5883Config) {
+        if (hmc5883Config->gpioAPB2Peripherals) {
+            RCC_APB2PeriphClockCmd(hmc5883Config->gpioAPB2Peripherals, ENABLE);
+        }
+
+        gpio.pin = hmc5883Config->gpioPin;
         gpio.speed = Speed_2MHz;
         gpio.mode = Mode_IN_FLOATING;
-        gpioInit(GPIOB, &gpio);
-    } else if (hse_value == 12000000) {
-        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
-        // PC14 - MAG_DRDY output on rev5 hardware
-        gpio.pin = Pin_14;
-        gpioInit(GPIOC, &gpio);
+        gpioInit(hmc5883Config->gpioPort, &gpio);
     }
-#endif
 
     delay(50);
     i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS);   // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h
index 52d2703333..0f2f78a503 100755
--- a/src/main/drivers/compass_hmc5883l.h
+++ b/src/main/drivers/compass_hmc5883l.h
@@ -17,6 +17,12 @@
 
 #pragma once
 
-bool hmc5883lDetect();
-void hmc5883lInit(void);
+typedef struct hmc5883Config_s {
+    uint32_t gpioAPB2Peripherals;
+    uint16_t gpioPin;
+    GPIO_TypeDef *gpioPort;
+} hmc5883Config_t;
+
+bool hmc5883lDetect(void);
+void hmc5883lInit(hmc5883Config_t *hmc5883Config);
 void hmc5883lRead(int16_t *magData);
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index 81d7b7b2aa..bac409617f 100644
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -34,6 +34,10 @@
 #include "sensors/sensors.h"
 #include "sensors/compass.h"
 
+#ifdef NAZE
+#include "hardware_revision.h"
+#endif
+
 extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.
 
 int16_t magADC[XYZ_AXIS_COUNT];
@@ -45,7 +49,27 @@ void compassInit(void)
 {
     // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
     LED1_ON;
-    hmc5883lInit();
+
+    hmc5883Config_t *hmc5883Config = 0;
+#ifdef NAZE
+    hmc5883Config_t nazeHmc5883Config;
+
+    if (hardwareRevision < NAZE32_REV5) {
+        nazeHmc5883Config.gpioAPB2Peripherals = RCC_APB2Periph_GPIOB;
+        nazeHmc5883Config.gpioPin = Pin_12;
+        nazeHmc5883Config.gpioPort = GPIOB;
+    } else {
+        nazeHmc5883Config.gpioAPB2Peripherals = RCC_APB2Periph_GPIOC;
+        nazeHmc5883Config.gpioPin = Pin_14;
+        nazeHmc5883Config.gpioPort = GPIOC;
+    }
+
+    hmc5883Config = &nazeHmc5883Config;
+#endif
+
+    hmc5883lInit(hmc5883Config);
+
+
     LED1_OFF;
     magInit = 1;
 }