NAZE32 - Add hardware revision detection.

Makefile

@@ -244,6 +244,7 @@ NAZE_SRC	 = startup_stm32f10x_md_gcc.S \
 		   drivers/sound_beeper_stm32f10x.c \
 		   drivers/system_stm32f10x.c \
 		   drivers/timer.c \
+		   hardware_revision.c \
 		   $(HIGHEND_SRC) \
 		   $(COMMON_SRC)
 

src/main/drivers/accgyro_spi_mpu6500.c

@@ -31,22 +31,6 @@
 #include "accgyro.h"
 #include "accgyro_spi_mpu6500.h"
 
-// MPU6500 (WHO_AM_I 0x70) on SPI bus
-
-#define MPU6500_RA_WHOAMI                   (0x75)
-#define MPU6500_RA_ACCEL_XOUT_H             (0x3B)
-#define MPU6500_RA_GYRO_XOUT_H              (0x43)
-#define MPU6500_RA_BANK_SEL                 (0x6D)
-#define MPU6500_RA_MEM_RW                   (0x6F)
-#define MPU6500_RA_GYRO_CFG                 (0x1B)
-#define MPU6500_RA_PWR_MGMT_1               (0x6B)
-#define MPU6500_RA_ACCEL_CFG                (0x1C)
-#define MPU6500_RA_LPF                      (0x1A)
-#define MPU6500_RA_RATE_DIV                 (0x19)
-
-#define MPU6500_WHO_AM_I_CONST              (0x70)
-#define BIT_RESET                           (0x80)
-
 enum lpf_e {
     INV_FILTER_256HZ_NOLPF2 = 0,
     INV_FILTER_188HZ,
@@ -192,7 +176,7 @@ static void mpu6500GyroInit(void)
     }
 #endif
 
-    mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, BIT_RESET);
+    mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
     delay(100);
     mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, 0);
     delay(100);

src/main/drivers/accgyro_spi_mpu6500.h

@@ -15,6 +15,20 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#define MPU6500_RA_WHOAMI                   (0x75)
+#define MPU6500_RA_ACCEL_XOUT_H             (0x3B)
+#define MPU6500_RA_GYRO_XOUT_H              (0x43)
+#define MPU6500_RA_BANK_SEL                 (0x6D)
+#define MPU6500_RA_MEM_RW                   (0x6F)
+#define MPU6500_RA_GYRO_CFG                 (0x1B)
+#define MPU6500_RA_PWR_MGMT_1               (0x6B)
+#define MPU6500_RA_ACCEL_CFG                (0x1C)
+#define MPU6500_RA_LPF                      (0x1A)
+#define MPU6500_RA_RATE_DIV                 (0x19)
+
+#define MPU6500_WHO_AM_I_CONST              (0x70)
+
+#define MPU6500_BIT_RESET                   (0x80)
 
 #pragma once
 

src/main/drivers/bus_spi.c

@@ -30,6 +30,7 @@ static volatile uint16_t spi2ErrorCount = 0;
 static volatile uint16_t spi3ErrorCount = 0;
 #endif
 
+#ifdef USE_SPI_DEVICE_1
 void initSpi1(void)
 {
     // Specific to the STM32F103
@@ -76,7 +77,9 @@ void initSpi1(void)
     SPI_Init(SPI1, &spi);
     SPI_Cmd(SPI1, ENABLE);
 }
+#endif
 
+#ifdef USE_SPI_DEVICE_2
 void initSpi2(void)
 {
 
@@ -108,6 +111,8 @@ void initSpi2(void)
     gpio.mode = Mode_Out_PP;
     gpioInit(GPIOB, &gpio);
 
+    GPIO_SetBits(GPIOB, GPIO_Pin_12);
+
     // Init SPI2 hardware
     SPI_I2S_DeInit(SPI2);
 
@@ -124,18 +129,23 @@ void initSpi2(void)
     SPI_Init(SPI2, &spi);
     SPI_Cmd(SPI2, ENABLE);
 }
+#endif
 
 bool spiInit(SPI_TypeDef *instance)
 {
+#ifdef USE_SPI_DEVICE_1
     if (instance == SPI1) {
         initSpi1();
-    } else if (instance == SPI2) {
+        return true;
-        initSpi2();
-    } else {
-        return false;
     }
-
+#endif
-    return true;
+#ifdef USE_SPI_DEVICE_2
+    if (instance == SPI1) {
+        initSpi2();
+        return true;
+    }
+#endif
+    return false;
 }
 
 uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance)

src/main/drivers/system.c

@@ -27,8 +27,6 @@
 #include "light_led.h"
 #include "sound_beeper.h"
 #include "inverter.h"
-#include "bus_i2c.h"
-#include "bus_spi.h"
 
 #include "system.h"
 
@@ -37,15 +35,6 @@ static volatile uint32_t usTicks = 0;
 // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
 static volatile uint32_t sysTickUptime = 0;
 
-#ifdef STM32F303xC
-// from system_stm32f30x.c
-void SetSysClock(void);
-#endif
-#ifdef STM32F10X
-// from system_stm32f10x.c
-void SetSysClock(bool overclock);
-#endif
-
 static void cycleCounterInit(void)
 {
     RCC_ClocksTypeDef clocks;
@@ -76,23 +65,8 @@ uint32_t millis(void)
     return sysTickUptime;
 }
 
-void systemInit(bool overclock)
+void systemInit(void)
 {
-
-#ifdef STM32F303
-    UNUSED(overclock);
-    // start fpu
-    SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
-#endif
-
-#ifdef STM32F303xC
-    SetSysClock();
-#endif
-#ifdef STM32F10X
-    // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
-    // Configure the Flash Latency cycles and enable prefetch buffer
-    SetSysClock(overclock);
-#endif
 #ifdef CC3D
     /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */
     extern void *isr_vector_table_base;
@@ -131,16 +105,6 @@ void systemInit(bool overclock)
     // SysTick
     SysTick_Config(SystemCoreClock / 1000);
 
-#ifdef USE_SPI
-    spiInit(SPI1);
-    spiInit(SPI2);
-#endif
-
-#ifdef USE_I2C
-    // Configure the rest of the stuff
-    i2cInit(I2C_DEVICE);
-#endif
-
     // sleep for 100ms
     delay(100);
 }

src/main/drivers/system.h

@@ -17,7 +17,7 @@
 
 #pragma once
 
-void systemInit(bool overclock);
+void systemInit(void);
 void delayMicroseconds(uint32_t us);
 void delay(uint32_t ms);
 

src/main/main.c

@@ -37,6 +37,8 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/pwm_rx.h"
 #include "drivers/adc.h"
+#include "drivers/bus_i2c.h"
+#include "drivers/bus_spi.h"
 
 #include "flight/flight.h"
 #include "flight/mixer.h"
@@ -66,6 +68,10 @@
 #include "config/config_profile.h"
 #include "config/config_master.h"
 
+#ifdef NAZE
+#include "target/NAZE/hardware_revision.h"
+#endif
+
 #include "build_config.h"
 
 #ifdef DEBUG_SECTION_TIMES
@@ -97,6 +103,16 @@ void displayInit(rxConfig_t *intialRxConfig);
 void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse);
 void loop(void);
 
+
+#ifdef STM32F303xC
+// from system_stm32f30x.c
+void SetSysClock(void);
+#endif
+#ifdef STM32F10X
+// from system_stm32f10x.c
+void SetSysClock(bool overclock);
+#endif
+
 // FIXME bad naming - this appears to be for some new board that hasn't been made available yet.
 #ifdef PROD_DEBUG
 void productionDebug(void)
@@ -133,7 +149,45 @@ void init(void)
     ensureEEPROMContainsValidData();
     readEEPROM();
 
-    systemInit(masterConfig.emf_avoidance);
+#ifdef STM32F303
+    // start fpu
+    SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
+#endif
+
+#ifdef STM32F303xC
+    SetSysClock();
+#endif
+#ifdef STM32F10X
+    // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
+    // Configure the Flash Latency cycles and enable prefetch buffer
+    SetSysClock(masterConfig.emf_avoidance);
+#endif
+
+#ifdef NAZE
+    detectHardwareRevision();
+#endif
+
+    systemInit();
+
+#ifdef USE_SPI
+    spiInit(SPI1);
+    spiInit(SPI2);
+#endif
+
+#ifdef NAZE
+    updateHardwareRevision();
+#endif
+
+#ifdef USE_I2C
+#ifdef NAZE
+    if (hardwareRevision != NAZE32_SP) {
+        i2cInit(I2C_DEVICE);
+    }
+#else
+    // Configure the rest of the stuff
+    i2cInit(I2C_DEVICE);
+#endif
+#endif
 
     adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
     adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);

src/main/target/CC3D/target.h

@@ -54,6 +54,9 @@
 #define SERIAL_PORT_COUNT 2
 
 #define USE_SPI
+#define USE_SPI_DEVICE_1
+#define USE_SPI_DEVICE_2
+
 
 #define SENSORS_SET (SENSOR_ACC)
 

src/main/target/NAZE/hardware_revision.c

@@ -0,0 +1,101 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include "platform.h"
+
+#include "build_config.h"
+
+#include "drivers/system.h"
+#include "drivers/bus_spi.h"
+#include "drivers/accgyro.h"
+#include "drivers/accgyro_spi_mpu6500.h"
+
+#include "hardware_revision.h"
+
+static const char * const hardwareRevisionNames[] = {
+        "Unknown",
+        "Naze 32",
+        "Naze32 rev.5",
+        "Naze32 SP"
+};
+
+uint8_t hardwareRevision = UNKNOWN;
+
+void detectHardwareRevision(void)
+{
+    if (hse_value == 8000000)
+        hardwareRevision = NAZE32;
+    else if (hse_value == 12000000)
+        hardwareRevision = NAZE32_REV5;
+}
+
+#ifdef USE_SPI
+
+#define DISABLE_SPI_CS       GPIO_SetBits(NAZE_SPI_CS_GPIO,   NAZE_SPI_CS_PIN)
+#define ENABLE_SPI_CS        GPIO_ResetBits(NAZE_SPI_CS_GPIO, NAZE_SPI_CS_PIN)
+
+#define SPI_DEVICE_NONE (0)
+#define SPI_DEVICE_FLASH (1)
+#define SPI_DEVICE_MPU (2)
+
+#define M25P16_INSTRUCTION_RDID 0x9F
+#define FLASH_M25P16 (0x202015)
+
+uint8_t detectSpiDevice(void)
+{
+    uint8_t out[] = { M25P16_INSTRUCTION_RDID, 0, 0, 0 };
+    uint8_t in[4];
+    uint32_t flash_id;
+
+    delay(50); // short delay required after initialisation of SPI device instance.
+
+    // try autodetect flash chip
+    ENABLE_SPI_CS;
+    spiTransfer(NAZE_SPI_INSTANCE, in, out, sizeof(out));
+    DISABLE_SPI_CS;
+
+    flash_id = in[1] << 16 | in[2] << 8 | in[3];
+    if (flash_id == FLASH_M25P16)
+        return SPI_DEVICE_FLASH;
+
+    // try autodetect MPU
+    ENABLE_SPI_CS;
+    spiTransferByte(NAZE_SPI_INSTANCE, MPU6500_RA_WHOAMI | MPU6500_BIT_RESET);
+    in[0] = spiTransferByte(NAZE_SPI_INSTANCE, 0xff);
+    DISABLE_SPI_CS;
+
+    if (in[0] == MPU6500_WHO_AM_I_CONST)
+        return SPI_DEVICE_MPU;
+
+    return SPI_DEVICE_NONE;
+}
+
+#endif
+
+void updateHardwareRevision(void)
+{
+#ifdef USE_SPI
+    uint8_t detectedSpiDevice = detectSpiDevice();
+
+    if (detectedSpiDevice == SPI_DEVICE_MPU && hardwareRevision == NAZE32_REV5)
+        hardwareRevision = NAZE32_SP;
+#endif
+}

src/main/target/NAZE/hardware_revision.h

@@ -0,0 +1,28 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+typedef enum nazeHardwareRevision_t {
+    UNKNOWN = 0,
+    NAZE32, // Naze32 and compatible with 8MHz HSE
+    NAZE32_REV5, // Naze32 and compatible with 12MHz HSE
+    NAZE32_SP // Naze32 w/Sensor Platforms
+} nazeHardwareRevision_e;
+
+extern uint8_t hardwareRevision;
+
+void updateHardwareRevision(void);
+void detectHardwareRevision(void);

src/main/target/NAZE/target.h

@@ -43,9 +43,16 @@
 // PB13 26 SPI2_SCK
 // PB12 25 SPI2_NSS
 
-#define MPU6500_CS_GPIO       GPIOB
+#define USE_SPI
-#define MPU6500_CS_PIN        GPIO_Pin_12
+#define USE_SPI_DEVICE_2
-#define MPU6500_SPI_INSTANCE  SPI2
+
+#define NAZE_SPI_INSTANCE     SPI2
+#define NAZE_SPI_CS_GPIO      GPIOB
+#define NAZE_SPI_CS_PIN       GPIO_Pin_12
+
+#define MPU6500_CS_GPIO       NAZE_SPI_CS_GPIO
+#define MPU6500_CS_PIN        NAZE_SPI_CS_PIN
+#define MPU6500_SPI_INSTANCE  NAZE_SPI_INSTANCE
 
 #define GYRO
 #define USE_GYRO_MPU6050
@@ -76,8 +83,6 @@
 #define USE_I2C
 #define I2C_DEVICE (I2CDEV_2)
 
-#define USE_SPI
-