From 0e8d375a1c00ef2ffc7cd1c27b25b2a70b784756 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 30 Jul 2016 11:27:04 +1000 Subject: [PATCH] Update to incorporate hardware revision detection for BJF4 target --- src/main/drivers/flash_m25p16.c | 20 ++++- src/main/drivers/flash_m25p16.h | 3 +- src/main/drivers/io.h | 3 + src/main/main.c | 11 ++- src/main/target/BLUEJAYF4/hardware_revision.c | 86 +++++++++++++++++++ src/main/target/BLUEJAYF4/hardware_revision.h | 30 +++++++ src/main/target/BLUEJAYF4/target.h | 11 ++- src/main/target/BLUEJAYF4/target.mk | 2 +- 8 files changed, 153 insertions(+), 13 deletions(-) create mode 100644 src/main/target/BLUEJAYF4/hardware_revision.c create mode 100644 src/main/target/BLUEJAYF4/hardware_revision.h diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 29620fbe11..d1e62cccbe 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -196,12 +196,26 @@ static bool m25p16_readIdentification() * Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with * m25p16_getGeometry(). */ -bool m25p16_init() +bool m25p16_init(ioTag_t csTag) { - + /* + if we have already detected a flash device we can simply exit + + TODO: change the init param in favour of flash CFG when ParamGroups work is done + then cs pin can be specified in hardware_revision.c or config.c (dependent on revision). + */ + if (geometry.sectors) { + return true; + } + + if (csTag) { + m25p16CsPin = IOGetByTag(csTag); + } else { #ifdef M25P16_CS_PIN - m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN)); + m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN)); +#else return false; #endif + } IOInit(m25p16CsPin, OWNER_FLASH, RESOURCE_SPI_CS, 0); IOConfigGPIO(m25p16CsPin, SPI_IO_CS_CFG); diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h index 972fe10bbc..223efa1809 100644 --- a/src/main/drivers/flash_m25p16.h +++ b/src/main/drivers/flash_m25p16.h @@ -19,10 +19,11 @@ #include #include "flash.h" +#include "io.h" #define M25P16_PAGESIZE 256 -bool m25p16_init(); +bool m25p16_init(ioTag_t csTag); void m25p16_eraseSector(uint32_t address); void m25p16_eraseCompletely(); diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index af9deca810..d655646282 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -11,6 +11,9 @@ typedef uint8_t ioTag_t; // packet tag to specify IO pin typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change +// NONE initializer for ioTag_t variables +#define IOTAG_NONE ((ioTag_t)0) + // NONE initializer for IO_t variable #define IO_NONE ((IO_t)0) diff --git a/src/main/main.c b/src/main/main.c index 1948a72ac3..7be831d73c 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -179,8 +179,7 @@ void init(void) #ifdef ALIENFLIGHTF3 if (hardwareRevision == AFF3_REV_1) { ledInit(false); - } - else { + } else { ledInit(true); } #else @@ -581,10 +580,10 @@ void init(void) #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { - m25p16_init(); + m25p16_init(IOTAG_NONE); } #elif defined(USE_FLASH_M25P16) - m25p16_init(); + m25p16_init(IOTAG_NONE); #endif flashfsInit(); @@ -599,7 +598,11 @@ void init(void) #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip +#ifdef STM32F4 + sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM; +#else sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; +#endif #else sdcardUseDMA = true; #endif diff --git a/src/main/target/BLUEJAYF4/hardware_revision.c b/src/main/target/BLUEJAYF4/hardware_revision.c new file mode 100644 index 0000000000..6ed07e16f9 --- /dev/null +++ b/src/main/target/BLUEJAYF4/hardware_revision.c @@ -0,0 +1,86 @@ +/* + * 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 . + */ + +#include +#include +#include + +#include "platform.h" +#include "build_config.h" + +#include "drivers/system.h" +#include "drivers/bus_spi.h" +#include "drivers/io.h" +#include "drivers/flash_m25p16.h" +#include "hardware_revision.h" + +static const char * const hardwareRevisionNames[] = { + "Unknown", + "BlueJay rev1", + "BlueJay rev2", + "BlueJay rev3", + "BlueJay rev3a" +}; + +uint8_t hardwareRevision = UNKNOWN; + +void detectHardwareRevision(void) +{ + IO_t pin1 = IOGetByTag(IO_TAG(PB12)); + IOInit(pin1, OWNER_SYSTEM, RESOURCE_INPUT, 1); + IOConfigGPIO(pin1, IOCFG_IPU); + + IO_t pin2 = IOGetByTag(IO_TAG(PB13)); + IOInit(pin2, OWNER_SYSTEM, RESOURCE_INPUT, 2); + IOConfigGPIO(pin2, IOCFG_IPU); + + // Check hardware revision + delayMicroseconds(10); // allow configuration to settle + + /* + if both PB12 and 13 are tied to GND then it is Rev3A (mini) + if only PB12 is tied to GND then it is a Rev3 (full size) + */ + if (!IORead(pin1)) { + if (!IORead(pin2)) { + hardwareRevision = BJF4_REV3A; + return; + } + hardwareRevision = BJF4_REV3; + return; + } + + hardwareRevision = BJF4_REV2; +} + +void updateHardwareRevision(void) +{ + if (hardwareRevision != BJF4_REV2) { + return; + } + + /* + if flash exists on PB3 then Rev1 + */ + if (m25p16_init(IO_TAG(PB3))) { + hardwareRevision = BJF4_REV1; + } else { + IOInit(IOGetByTag(IO_TAG(PB3)), OWNER_FREE, RESOURCE_NONE, 0); + } +} + + diff --git a/src/main/target/BLUEJAYF4/hardware_revision.h b/src/main/target/BLUEJAYF4/hardware_revision.h new file mode 100644 index 0000000000..3333d86aa0 --- /dev/null +++ b/src/main/target/BLUEJAYF4/hardware_revision.h @@ -0,0 +1,30 @@ +/* + * 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 . + */ +#pragma once + +typedef enum bjf4HardwareRevision_t { + UNKNOWN = 0, + BJF4_REV1, // Flash + BJF4_REV2, // SDCard + BJF4_REV3, // SDCard + Flash + BJF4_REV3A, // Flash (20x20 mini format) +} bjf4HardwareRevision_e; + +extern uint8_t hardwareRevision; + +void updateHardwareRevision(void); +void detectHardwareRevision(void); \ No newline at end of file diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 34bdcf8aa7..cea7bb5b46 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -22,6 +22,9 @@ #define USBD_PRODUCT_STRING "BlueJayF4" +#define USE_HARDWARE_REVISION_DETECTION +#define HW_PIN PB2 + #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_EXTI @@ -76,11 +79,11 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -//#define M25P16_CS_PIN PB3 -//#define M25P16_SPI_INSTANCE SPI3 +#define M25P16_CS_PIN PB7 +#define M25P16_SPI_INSTANCE SPI3 -//#define USE_FLASHFS -//#define USE_FLASH_M25P16 +#define USE_FLASHFS +#define USE_FLASH_M25P16 #define USABLE_TIMER_CHANNEL_COUNT 7 diff --git a/src/main/target/BLUEJAYF4/target.mk b/src/main/target/BLUEJAYF4/target.mk index 23dd74507a..d0a982faad 100644 --- a/src/main/target/BLUEJAYF4/target.mk +++ b/src/main/target/BLUEJAYF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_spi_mpu6500.c \