diff --git a/Makefile b/Makefile
index df4afde79b..699227f4c0 100644
--- a/Makefile
+++ b/Makefile
@@ -567,6 +567,7 @@ STM32F3DISCOVERY_SRC = \
drivers/barometer_ms5611.c \
drivers/barometer_bmp280.c \
drivers/compass_ak8975.c \
+ drivers/sdcard.c \
$(HIGHEND_SRC) \
$(COMMON_SRC)
diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c
new file mode 100644
index 0000000000..8dbb4c7073
--- /dev/null
+++ b/src/main/drivers/sdcard.c
@@ -0,0 +1,826 @@
+/*
+ * 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 "platform.h"
+
+#include "drivers/gpio.h"
+#include "drivers/sdcard.h"
+
+void SD_Detect_LowLevel_DeInit(void)
+{
+ GPIO_InitTypeDef GPIO_InitStructure;
+
+ /* Configure SD_SPI_INSTANCE_DETECT_PIN pin: SD Card detect pin */
+ GPIO_InitStructure.GPIO_Pin = SD_DETECT_PIN;
+ GPIO_Init(SD_DETECT_GPIO_PORT, &GPIO_InitStructure);
+}
+
+void SD_Detect_LowLevel_Init(void)
+{
+ GPIO_InitTypeDef GPIO_InitStructure;
+
+ /* Configure SD_SPI_INSTANCE_DETECT_PIN pin: SD Card detect pin */
+ GPIO_InitStructure.GPIO_Pin = SD_DETECT_PIN;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+ GPIO_Init(SD_DETECT_GPIO_PORT, &GPIO_InitStructure);
+}
+
+/**
+ * @brief DeInitializes the SD/SD communication.
+ * @param None
+ * @retval None
+ */
+void SD_DeInit(void)
+{
+ //SD_LowLevel_DeInit();
+}
+
+/**
+ * @brief Initializes the SD/SD communication.
+ * @param None
+ * @retval The SD Response:
+ * - SD_RESPONSE_FAILURE: Sequence failed
+ * - SD_RESPONSE_NO_ERROR: Sequence succeed
+ */
+SD_Error SD_Init(void)
+{
+ uint32_t i = 0;
+
+ /*!< Initialize SD_SPI_INSTANCE */
+ //SD_LowLevel_Init();
+
+ /*!< SD chip select high */
+ SD_CS_HIGH();
+
+ /*!< Send dummy byte 0xFF, 10 times with CS high */
+ /*!< Rise CS and MOSI for 80 clocks cycles */
+ for (i = 0; i <= 9; i++)
+ {
+ /*!< Send dummy byte 0xFF */
+ SD_WriteByte(SD_DUMMY_BYTE);
+ }
+
+ /*------------Put SD in SPI mode--------------*/
+ /*!< SD initialized and set to SPI mode properly */
+ return (SD_GoIdleState());
+}
+
+/**
+ * @brief Detect if SD card is correctly plugged in the memory slot.
+ * @param None
+ * @retval Return if SD is detected or not
+ */
+uint8_t SD_Detect(void)
+{
+ __IO uint8_t status = SD_NOT_PRESENT;
+
+ /*!< Check GPIO to detect SD */
+ if (GPIO_ReadInputData(SD_DETECT_GPIO_PORT) & SD_DETECT_PIN)
+ {
+ status = SD_PRESENT;
+ }
+ return status;
+}
+
+
+/**
+ * @brief Returns information about specific card.
+ * @param cardinfo: pointer to a SD_CardInfo structure that contains all SD
+ * card information.
+ * @retval The SD Response:
+ * - SD_RESPONSE_FAILURE: Sequence failed
+ * - SD_RESPONSE_NO_ERROR: Sequence succeed
+ */
+SD_Error SD_GetCardInfo(SD_CardInfo *cardinfo)
+{
+ SD_Error status = SD_RESPONSE_FAILURE;
+
+ SD_GetCSDRegister(&(cardinfo->SD_csd));
+ status = SD_GetCIDRegister(&(cardinfo->SD_cid));
+ cardinfo->CardCapacity = (cardinfo->SD_csd.DeviceSize + 1) ;
+ cardinfo->CardCapacity *= (1 << (cardinfo->SD_csd.DeviceSizeMul + 2));
+ cardinfo->CardBlockSize = 1 << (cardinfo->SD_csd.RdBlockLen);
+ cardinfo->CardCapacity *= cardinfo->CardBlockSize;
+
+ /*!< Returns the reponse */
+ return status;
+}
+
+/**
+ * @brief Reads a block of data from the SD.
+ * @param pBuffer: pointer to the buffer that receives the data read from the
+ * SD.
+ * @param ReadAddr: SD's internal address to read from.
+ * @param BlockSize: the SD card Data block size.
+ * @retval The SD Response:
+ * - SD_RESPONSE_FAILURE: Sequence failed
+ * - SD_RESPONSE_NO_ERROR: Sequence succeed
+ */
+SD_Error SD_ReadBlock(uint8_t* pBuffer, uint32_t ReadAddr, uint16_t BlockSize)
+{
+ uint32_t i = 0;
+ SD_Error rvalue = SD_RESPONSE_FAILURE;
+
+ /*!< SD chip select low */
+ SD_CS_LOW();
+
+ /*!< Send CMD17 (SD_CMD_READ_SINGLE_BLOCK) to read one block */
+ SD_SendCmd(SD_CMD_READ_SINGLE_BLOCK, ReadAddr, 0xFF);
+
+ /*!< Check if the SD acknowledged the read block command: R1 response (0x00: no errors) */
+ if (!SD_GetResponse(SD_RESPONSE_NO_ERROR))
+ {
+ /*!< Now look for the data token to signify the start of the data */
+ if (!SD_GetResponse(SD_START_DATA_SINGLE_BLOCK_READ))
+ {
+ /*!< Read the SD block data : read NumByteToRead data */
+ for (i = 0; i < BlockSize; i++)
+ {
+ /*!< Save the received data */
+ *pBuffer = SD_ReadByte();
+
+ /*!< Point to the next location where the byte read will be saved */
+ pBuffer++;
+ }
+ /*!< Get CRC bytes (not really needed by us, but required by SD) */
+ SD_ReadByte();
+ SD_ReadByte();
+ /*!< Set response value to success */
+ rvalue = SD_RESPONSE_NO_ERROR;
+ }
+ }
+ /*!< SD chip select high */
+ SD_CS_HIGH();
+
+ /*!< Send dummy byte: 8 Clock pulses of delay */
+ SD_WriteByte(SD_DUMMY_BYTE);
+
+ /*!< Returns the reponse */
+ return rvalue;
+}
+
+/**
+ * @brief Reads multiple block of data from the SD.
+ * @param pBuffer: pointer to the buffer that receives the data read from the
+ * SD.
+ * @param ReadAddr: SD's internal address to read from.
+ * @param BlockSize: the SD card Data block size.
+ * @param NumberOfBlocks: number of blocks to be read.
+ * @retval The SD Response:
+ * - SD_RESPONSE_FAILURE: Sequence failed
+ * - SD_RESPONSE_NO_ERROR: Sequence succeed
+ */
+SD_Error SD_ReadMultiBlocks(uint8_t* pBuffer, uint32_t ReadAddr, uint16_t BlockSize, uint32_t NumberOfBlocks)
+{
+ uint32_t i = 0, Offset = 0;
+ SD_Error rvalue = SD_RESPONSE_FAILURE;
+
+ /*!< SD chip select low */
+ SD_CS_LOW();
+ /*!< Data transfer */
+ while (NumberOfBlocks--)
+ {
+ /*!< Send CMD17 (SD_CMD_READ_SINGLE_BLOCK) to read one block */
+ SD_SendCmd (SD_CMD_READ_SINGLE_BLOCK, ReadAddr + Offset, 0xFF);
+ /*!< Check if the SD acknowledged the read block command: R1 response (0x00: no errors) */
+ if (SD_GetResponse(SD_RESPONSE_NO_ERROR))
+ {
+ return SD_RESPONSE_FAILURE;
+ }
+ /*!< Now look for the data token to signify the start of the data */
+ if (!SD_GetResponse(SD_START_DATA_SINGLE_BLOCK_READ))
+ {
+ /*!< Read the SD block data : read NumByteToRead data */
+ for (i = 0; i < BlockSize; i++)
+ {
+ /*!< Read the pointed data */
+ *pBuffer = SD_ReadByte();
+ /*!< Point to the next location where the byte read will be saved */
+ pBuffer++;
+ }
+ /*!< Set next read address*/
+ Offset += 512;
+ /*!< get CRC bytes (not really needed by us, but required by SD) */
+ SD_ReadByte();
+ SD_ReadByte();
+ /*!< Set response value to success */
+ rvalue = SD_RESPONSE_NO_ERROR;
+ }
+ else
+ {
+ /*!< Set response value to failure */
+ rvalue = SD_RESPONSE_FAILURE;
+ }
+ }
+ /*!< SD chip select high */
+ SD_CS_HIGH();
+ /*!< Send dummy byte: 8 Clock pulses of delay */
+ SD_WriteByte(SD_DUMMY_BYTE);
+ /*!< Returns the reponse */
+ return rvalue;
+}
+
+/**
+ * @brief Writes a block on the SD
+ * @param pBuffer: pointer to the buffer containing the data to be written on
+ * the SD.
+ * @param WriteAddr: address to write on.
+ * @param BlockSize: the SD card Data block size.
+ * @retval The SD Response:
+ * - SD_RESPONSE_FAILURE: Sequence failed
+ * - SD_RESPONSE_NO_ERROR: Sequence succeed
+ */
+SD_Error SD_WriteBlock(uint8_t* pBuffer, uint32_t WriteAddr, uint16_t BlockSize)
+{
+ uint32_t i = 0;
+ SD_Error rvalue = SD_RESPONSE_FAILURE;
+
+ /*!< SD chip select low */
+ SD_CS_LOW();
+
+ /*!< Send CMD24 (SD_CMD_WRITE_SINGLE_BLOCK) to write multiple block */
+ SD_SendCmd(SD_CMD_WRITE_SINGLE_BLOCK, WriteAddr, 0xFF);
+
+ /*!< Check if the SD acknowledged the write block command: R1 response (0x00: no errors) */
+ if (!SD_GetResponse(SD_RESPONSE_NO_ERROR))
+ {
+ /*!< Send a dummy byte */
+ SD_WriteByte(SD_DUMMY_BYTE);
+
+ /*!< Send the data token to signify the start of the data */
+ SD_WriteByte(0xFE);
+
+ /*!< Write the block data to SD : write count data by block */
+ for (i = 0; i < BlockSize; i++)
+ {
+ /*!< Send the pointed byte */
+ SD_WriteByte(*pBuffer);
+ /*!< Point to the next location where the byte read will be saved */
+ pBuffer++;
+ }
+ /*!< Put CRC bytes (not really needed by us, but required by SD) */
+ SD_ReadByte();
+ SD_ReadByte();
+ /*!< Read data response */
+ if (SD_GetDataResponse() == SD_DATA_OK)
+ {
+ rvalue = SD_RESPONSE_NO_ERROR;
+ }
+ }
+ /*!< SD chip select high */
+ SD_CS_HIGH();
+ /*!< Send dummy byte: 8 Clock pulses of delay */
+ SD_WriteByte(SD_DUMMY_BYTE);
+
+ /*!< Returns the reponse */
+ return rvalue;
+}
+
+/**
+ * @brief Writes many blocks on the SD
+ * @param pBuffer: pointer to the buffer containing the data to be written on
+ * the SD.
+ * @param WriteAddr: address to write on.
+ * @param BlockSize: the SD card Data block size.
+ * @param NumberOfBlocks: number of blocks to be written.
+ * @retval The SD Response:
+ * - SD_RESPONSE_FAILURE: Sequence failed
+ * - SD_RESPONSE_NO_ERROR: Sequence succeed
+ */
+SD_Error SD_WriteMultiBlocks(uint8_t* pBuffer, uint32_t WriteAddr, uint16_t BlockSize, uint32_t NumberOfBlocks)
+{
+ uint32_t i = 0, Offset = 0;
+ SD_Error rvalue = SD_RESPONSE_FAILURE;
+
+ /*!< SD chip select low */
+ SD_CS_LOW();
+ /*!< Data transfer */
+ while (NumberOfBlocks--)
+ {
+ /*!< Send CMD24 (SD_CMD_WRITE_SINGLE_BLOCK) to write blocks */
+ SD_SendCmd(SD_CMD_WRITE_SINGLE_BLOCK, WriteAddr + Offset, 0xFF);
+ /*!< Check if the SD acknowledged the write block command: R1 response (0x00: no errors) */
+ if (SD_GetResponse(SD_RESPONSE_NO_ERROR))
+ {
+ return SD_RESPONSE_FAILURE;
+ }
+ /*!< Send dummy byte */
+ SD_WriteByte(SD_DUMMY_BYTE);
+ /*!< Send the data token to signify the start of the data */
+ SD_WriteByte(SD_START_DATA_SINGLE_BLOCK_WRITE);
+ /*!< Write the block data to SD : write count data by block */
+ for (i = 0; i < BlockSize; i++)
+ {
+ /*!< Send the pointed byte */
+ SD_WriteByte(*pBuffer);
+ /*!< Point to the next location where the byte read will be saved */
+ pBuffer++;
+ }
+ /*!< Set next write address */
+ Offset += 512;
+ /*!< Put CRC bytes (not really needed by us, but required by SD) */
+ SD_ReadByte();
+ SD_ReadByte();
+ /*!< Read data response */
+ if (SD_GetDataResponse() == SD_DATA_OK)
+ {
+ /*!< Set response value to success */
+ rvalue = SD_RESPONSE_NO_ERROR;
+ }
+ else
+ {
+ /*!< Set response value to failure */
+ rvalue = SD_RESPONSE_FAILURE;
+ }
+ }
+ /*!< SD chip select high */
+ SD_CS_HIGH();
+ /*!< Send dummy byte: 8 Clock pulses of delay */
+ SD_WriteByte(SD_DUMMY_BYTE);
+ /*!< Returns the reponse */
+ return rvalue;
+}
+
+/**
+ * @brief Read the CSD card register.
+ * Reading the contents of the CSD register in SPI mode is a simple
+ * read-block transaction.
+ * @param SD_csd: pointer on an SCD register structure
+ * @retval The SD Response:
+ * - SD_RESPONSE_FAILURE: Sequence failed
+ * - SD_RESPONSE_NO_ERROR: Sequence succeed
+ */
+SD_Error SD_GetCSDRegister(SD_CSD* SD_csd)
+{
+ uint32_t i = 0;
+ SD_Error rvalue = SD_RESPONSE_FAILURE;
+ uint8_t CSD_Tab[16];
+
+ /*!< SD chip select low */
+ SD_CS_LOW();
+ /*!< Send CMD9 (CSD register) or CMD10(CSD register) */
+ SD_SendCmd(SD_CMD_SEND_CSD, 0, 0xFF);
+ /*!< Wait for response in the R1 format (0x00 is no errors) */
+ if (!SD_GetResponse(SD_RESPONSE_NO_ERROR))
+ {
+ if (!SD_GetResponse(SD_START_DATA_SINGLE_BLOCK_READ))
+ {
+ for (i = 0; i < 16; i++)
+ {
+ /*!< Store CSD register value on CSD_Tab */
+ CSD_Tab[i] = SD_ReadByte();
+ }
+ }
+ /*!< Get CRC bytes (not really needed by us, but required by SD) */
+ SD_WriteByte(SD_DUMMY_BYTE);
+ SD_WriteByte(SD_DUMMY_BYTE);
+ /*!< Set response value to success */
+ rvalue = SD_RESPONSE_NO_ERROR;
+ }
+ /*!< SD chip select high */
+ SD_CS_HIGH();
+ /*!< Send dummy byte: 8 Clock pulses of delay */
+ SD_WriteByte(SD_DUMMY_BYTE);
+
+ /*!< Byte 0 */
+ SD_csd->CSDStruct = (CSD_Tab[0] & 0xC0) >> 6;
+ SD_csd->SysSpecVersion = (CSD_Tab[0] & 0x3C) >> 2;
+ SD_csd->Reserved1 = CSD_Tab[0] & 0x03;
+
+ /*!< Byte 1 */
+ SD_csd->TAAC = CSD_Tab[1];
+
+ /*!< Byte 2 */
+ SD_csd->NSAC = CSD_Tab[2];
+
+ /*!< Byte 3 */
+ SD_csd->MaxBusClkFrec = CSD_Tab[3];
+
+ /*!< Byte 4 */
+ SD_csd->CardComdClasses = CSD_Tab[4] << 4;
+
+ /*!< Byte 5 */
+ SD_csd->CardComdClasses |= (CSD_Tab[5] & 0xF0) >> 4;
+ SD_csd->RdBlockLen = CSD_Tab[5] & 0x0F;
+
+ /*!< Byte 6 */
+ SD_csd->PartBlockRead = (CSD_Tab[6] & 0x80) >> 7;
+ SD_csd->WrBlockMisalign = (CSD_Tab[6] & 0x40) >> 6;
+ SD_csd->RdBlockMisalign = (CSD_Tab[6] & 0x20) >> 5;
+ SD_csd->DSRImpl = (CSD_Tab[6] & 0x10) >> 4;
+ SD_csd->Reserved2 = 0; /*!< Reserved */
+
+ SD_csd->DeviceSize = (CSD_Tab[6] & 0x03) << 10;
+
+ /*!< Byte 7 */
+ SD_csd->DeviceSize |= (CSD_Tab[7]) << 2;
+
+ /*!< Byte 8 */
+ SD_csd->DeviceSize |= (CSD_Tab[8] & 0xC0) >> 6;
+
+ SD_csd->MaxRdCurrentVDDMin = (CSD_Tab[8] & 0x38) >> 3;
+ SD_csd->MaxRdCurrentVDDMax = (CSD_Tab[8] & 0x07);
+
+ /*!< Byte 9 */
+ SD_csd->MaxWrCurrentVDDMin = (CSD_Tab[9] & 0xE0) >> 5;
+ SD_csd->MaxWrCurrentVDDMax = (CSD_Tab[9] & 0x1C) >> 2;
+ SD_csd->DeviceSizeMul = (CSD_Tab[9] & 0x03) << 1;
+ /*!< Byte 10 */
+ SD_csd->DeviceSizeMul |= (CSD_Tab[10] & 0x80) >> 7;
+
+ SD_csd->EraseGrSize = (CSD_Tab[10] & 0x40) >> 6;
+ SD_csd->EraseGrMul = (CSD_Tab[10] & 0x3F) << 1;
+
+ /*!< Byte 11 */
+ SD_csd->EraseGrMul |= (CSD_Tab[11] & 0x80) >> 7;
+ SD_csd->WrProtectGrSize = (CSD_Tab[11] & 0x7F);
+
+ /*!< Byte 12 */
+ SD_csd->WrProtectGrEnable = (CSD_Tab[12] & 0x80) >> 7;
+ SD_csd->ManDeflECC = (CSD_Tab[12] & 0x60) >> 5;
+ SD_csd->WrSpeedFact = (CSD_Tab[12] & 0x1C) >> 2;
+ SD_csd->MaxWrBlockLen = (CSD_Tab[12] & 0x03) << 2;
+
+ /*!< Byte 13 */
+ SD_csd->MaxWrBlockLen |= (CSD_Tab[13] & 0xC0) >> 6;
+ SD_csd->WriteBlockPaPartial = (CSD_Tab[13] & 0x20) >> 5;
+ SD_csd->Reserved3 = 0;
+ SD_csd->ContentProtectAppli = (CSD_Tab[13] & 0x01);
+
+ /*!< Byte 14 */
+ SD_csd->FileFormatGrouop = (CSD_Tab[14] & 0x80) >> 7;
+ SD_csd->CopyFlag = (CSD_Tab[14] & 0x40) >> 6;
+ SD_csd->PermWrProtect = (CSD_Tab[14] & 0x20) >> 5;
+ SD_csd->TempWrProtect = (CSD_Tab[14] & 0x10) >> 4;
+ SD_csd->FileFormat = (CSD_Tab[14] & 0x0C) >> 2;
+ SD_csd->ECC = (CSD_Tab[14] & 0x03);
+
+ /*!< Byte 15 */
+ SD_csd->CSD_CRC = (CSD_Tab[15] & 0xFE) >> 1;
+ SD_csd->Reserved4 = 1;
+
+ /*!< Return the reponse */
+ return rvalue;
+}
+
+/**
+ * @brief Read the CID card register.
+ * Reading the contents of the CID register in SPI mode is a simple
+ * read-block transaction.
+ * @param SD_cid: pointer on an CID register structure
+ * @retval The SD Response:
+ * - SD_RESPONSE_FAILURE: Sequence failed
+ * - SD_RESPONSE_NO_ERROR: Sequence succeed
+ */
+SD_Error SD_GetCIDRegister(SD_CID* SD_cid)
+{
+ uint32_t i = 0;
+ SD_Error rvalue = SD_RESPONSE_FAILURE;
+ uint8_t CID_Tab[16];
+
+ /*!< SD chip select low */
+ SD_CS_LOW();
+
+ /*!< Send CMD10 (CID register) */
+ SD_SendCmd(SD_CMD_SEND_CID, 0, 0xFF);
+
+ /*!< Wait for response in the R1 format (0x00 is no errors) */
+ if (!SD_GetResponse(SD_RESPONSE_NO_ERROR))
+ {
+ if (!SD_GetResponse(SD_START_DATA_SINGLE_BLOCK_READ))
+ {
+ /*!< Store CID register value on CID_Tab */
+ for (i = 0; i < 16; i++)
+ {
+ CID_Tab[i] = SD_ReadByte();
+ }
+ }
+ /*!< Get CRC bytes (not really needed by us, but required by SD) */
+ SD_WriteByte(SD_DUMMY_BYTE);
+ SD_WriteByte(SD_DUMMY_BYTE);
+ /*!< Set response value to success */
+ rvalue = SD_RESPONSE_NO_ERROR;
+ }
+ /*!< SD chip select high */
+ SD_CS_HIGH();
+ /*!< Send dummy byte: 8 Clock pulses of delay */
+ SD_WriteByte(SD_DUMMY_BYTE);
+
+ /*!< Byte 0 */
+ SD_cid->ManufacturerID = CID_Tab[0];
+
+ /*!< Byte 1 */
+ SD_cid->OEM_AppliID = CID_Tab[1] << 8;
+
+ /*!< Byte 2 */
+ SD_cid->OEM_AppliID |= CID_Tab[2];
+
+ /*!< Byte 3 */
+ SD_cid->ProdName1 = CID_Tab[3] << 24;
+
+ /*!< Byte 4 */
+ SD_cid->ProdName1 |= CID_Tab[4] << 16;
+
+ /*!< Byte 5 */
+ SD_cid->ProdName1 |= CID_Tab[5] << 8;
+
+ /*!< Byte 6 */
+ SD_cid->ProdName1 |= CID_Tab[6];
+
+ /*!< Byte 7 */
+ SD_cid->ProdName2 = CID_Tab[7];
+
+ /*!< Byte 8 */
+ SD_cid->ProdRev = CID_Tab[8];
+
+ /*!< Byte 9 */
+ SD_cid->ProdSN = CID_Tab[9] << 24;
+
+ /*!< Byte 10 */
+ SD_cid->ProdSN |= CID_Tab[10] << 16;
+
+ /*!< Byte 11 */
+ SD_cid->ProdSN |= CID_Tab[11] << 8;
+
+ /*!< Byte 12 */
+ SD_cid->ProdSN |= CID_Tab[12];
+
+ /*!< Byte 13 */
+ SD_cid->Reserved1 |= (CID_Tab[13] & 0xF0) >> 4;
+ SD_cid->ManufactDate = (CID_Tab[13] & 0x0F) << 8;
+
+ /*!< Byte 14 */
+ SD_cid->ManufactDate |= CID_Tab[14];
+
+ /*!< Byte 15 */
+ SD_cid->CID_CRC = (CID_Tab[15] & 0xFE) >> 1;
+ SD_cid->Reserved2 = 1;
+
+ /*!< Return the reponse */
+ return rvalue;
+}
+
+/**
+ * @brief Send 5 bytes command to the SD card.
+ * @param Cmd: The user expected command to send to SD card.
+ * @param Arg: The command argument.
+ * @param Crc: The CRC.
+ * @retval None
+ */
+void SD_SendCmd(uint8_t Cmd, uint32_t Arg, uint8_t Crc)
+{
+ uint32_t i = 0x00;
+
+ uint8_t Frame[6];
+
+ Frame[0] = (Cmd | 0x40); /*!< Construct byte 1 */
+
+ Frame[1] = (uint8_t)(Arg >> 24); /*!< Construct byte 2 */
+
+ Frame[2] = (uint8_t)(Arg >> 16); /*!< Construct byte 3 */
+
+ Frame[3] = (uint8_t)(Arg >> 8); /*!< Construct byte 4 */
+
+ Frame[4] = (uint8_t)(Arg); /*!< Construct byte 5 */
+
+ Frame[5] = (Crc); /*!< Construct CRC: byte 6 */
+
+ for (i = 0; i < 6; i++)
+ {
+ SD_WriteByte(Frame[i]); /*!< Send the Cmd bytes */
+ }
+}
+
+/**
+ * @brief Get SD card data response.
+ * @param None
+ * @retval The SD status: Read data response xxx01
+ * - status 010: Data accepted
+ * - status 101: Data rejected due to a crc error
+ * - status 110: Data rejected due to a Write error.
+ * - status 111: Data rejected due to other error.
+ */
+uint8_t SD_GetDataResponse(void)
+{
+ uint32_t i = 0;
+ uint8_t response, rvalue;
+
+ while (i <= 64)
+ {
+ /*!< Read response */
+ response = SD_ReadByte();
+ /*!< Mask unused bits */
+ response &= 0x1F;
+ switch (response)
+ {
+ case SD_DATA_OK:
+ {
+ rvalue = SD_DATA_OK;
+ break;
+ }
+ case SD_DATA_CRC_ERROR:
+ return SD_DATA_CRC_ERROR;
+ case SD_DATA_WRITE_ERROR:
+ return SD_DATA_WRITE_ERROR;
+ default:
+ {
+ rvalue = SD_DATA_OTHER_ERROR;
+ break;
+ }
+ }
+ /*!< Exit loop in case of data ok */
+ if (rvalue == SD_DATA_OK)
+ break;
+ /*!< Increment loop counter */
+ i++;
+ }
+
+ /*!< Wait null data */
+ while (SD_ReadByte() == 0);
+
+ /*!< Return response */
+ return response;
+}
+
+/**
+ * @brief Returns the SD response.
+ * @param None
+ * @retval The SD Response:
+ * - SD_RESPONSE_FAILURE: Sequence failed
+ * - SD_RESPONSE_NO_ERROR: Sequence succeed
+ */
+SD_Error SD_GetResponse(uint8_t Response)
+{
+ uint32_t Count = 0xFFF;
+
+ /* Check if response is got or a timeout is happen */
+ while ((SD_ReadByte() != Response) && Count)
+ {
+ Count--;
+ }
+
+ if (Count == 0)
+ {
+ /* After time out */
+ return SD_RESPONSE_FAILURE;
+ }
+ else
+ {
+ /* Right response got */
+ return SD_RESPONSE_NO_ERROR;
+ }
+}
+
+/**
+ * @brief Returns the SD status.
+ * @param None
+ * @retval The SD status.
+ */
+uint16_t SD_GetStatus(void)
+{
+ uint16_t Status = 0;
+
+ /*!< SD chip select low */
+ SD_CS_LOW();
+
+ /*!< Send CMD13 (SD_SEND_STATUS) to get SD status */
+ SD_SendCmd(SD_CMD_SEND_STATUS, 0, 0xFF);
+
+ Status = SD_ReadByte();
+ Status |= (uint16_t)(SD_ReadByte() << 8);
+
+ /*!< SD chip select high */
+ SD_CS_HIGH();
+
+ /*!< Send dummy byte 0xFF */
+ SD_WriteByte(SD_DUMMY_BYTE);
+
+ return Status;
+}
+
+/**
+ * @brief Put SD in Idle state.
+ * @param None
+ * @retval The SD Response:
+ * - SD_RESPONSE_FAILURE: Sequence failed
+ * - SD_RESPONSE_NO_ERROR: Sequence succeed
+ */
+SD_Error SD_GoIdleState(void)
+{
+ /*!< SD chip select low */
+ SD_CS_LOW();
+
+ /*!< Send CMD0 (SD_CMD_GO_IDLE_STATE) to put SD in SPI mode */
+ SD_SendCmd(SD_CMD_GO_IDLE_STATE, 0, 0x95);
+
+ /*!< Wait for In Idle State Response (R1 Format) equal to 0x01 */
+ if (SD_GetResponse(SD_IN_IDLE_STATE))
+ {
+ /*!< No Idle State Response: return response failue */
+ return SD_RESPONSE_FAILURE;
+ }
+
+// SD_SendCmd(SD_CMD_SEND_IF_COND, 0, 0x65);
+// /*!< Wait for In Idle State Response (R1 Format) equal to 0x01 */
+// if (SD_GetResponse(SD_IN_IDLE_STATE))
+// {
+// /*!< No Idle State Response: return response failue */
+// return SD_RESPONSE_FAILURE;
+// }
+
+
+ /*----------Activates the card initialization process-----------*/
+ do
+ {
+ /*!< SD chip select high */
+ SD_CS_HIGH();
+
+ /*!< Send Dummy byte 0xFF */
+ SD_WriteByte(SD_DUMMY_BYTE);
+
+ /*!< SD chip select low */
+ SD_CS_LOW();
+
+ /*!< Send CMD1 (Activates the card process) until response equal to 0x0 */
+ SD_SendCmd(SD_CMD_SEND_OP_COND, 0, 0xFF);
+ /*!< Wait for no error Response (R1 Format) equal to 0x00 */
+ }
+ while (SD_GetResponse(SD_RESPONSE_NO_ERROR));
+
+ /*!< SD chip select high */
+ SD_CS_HIGH();
+
+ /*!< Send dummy byte 0xFF */
+ SD_WriteByte(SD_DUMMY_BYTE);
+
+ return SD_RESPONSE_NO_ERROR;
+}
+
+/**
+ * @brief Write a byte on the SD.
+ * @param Data: byte to send.
+ * @retval None
+ */
+uint8_t SD_WriteByte(uint8_t Data)
+{
+ /*!< Wait until the transmit buffer is empty */
+ while(SPI_I2S_GetFlagStatus(SD_SPI_INSTANCE, SPI_I2S_FLAG_TXE) == RESET)
+ {
+ }
+
+ /*!< Send the byte */
+ SPI_SendData8(SD_SPI_INSTANCE, Data);
+
+ /*!< Wait to receive a byte*/
+ while(SPI_I2S_GetFlagStatus(SD_SPI_INSTANCE, SPI_I2S_FLAG_RXNE) == RESET)
+ {
+ }
+
+ /*!< Return the byte read from the SPI bus */
+ return SPI_ReceiveData8(SD_SPI_INSTANCE);
+}
+
+/**
+ * @brief Read a byte from the SD.
+ * @param None
+ * @retval The received byte.
+ */
+uint8_t SD_ReadByte(void)
+{
+ uint8_t Data = 0;
+
+ /*!< Wait until the transmit buffer is empty */
+ while (SPI_I2S_GetFlagStatus(SD_SPI_INSTANCE, SPI_I2S_FLAG_TXE) == RESET)
+ {
+ }
+ /*!< Send the byte */
+ SPI_SendData8(SD_SPI_INSTANCE, SD_DUMMY_BYTE);
+
+ /*!< Wait until a data is received */
+ while (SPI_I2S_GetFlagStatus(SD_SPI_INSTANCE, SPI_I2S_FLAG_RXNE) == RESET)
+ {
+ }
+ /*!< Get the received data */
+ Data = SPI_ReceiveData8(SD_SPI_INSTANCE);
+
+ /*!< Return the shifted data */
+ return Data;
+}
diff --git a/src/main/drivers/sdcard.h b/src/main/drivers/sdcard.h
new file mode 100644
index 0000000000..b39d22f1ab
--- /dev/null
+++ b/src/main/drivers/sdcard.h
@@ -0,0 +1,223 @@
+/*
+ * 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 .
+ */
+
+void SD_Detect_LowLevel_DeInit(void);
+void SD_Detect_LowLevel_Init(void);
+
+
+/** @defgroup STM32303C_EVAL_SPI_SD_Exported_Types
+ * @{
+ */
+
+typedef enum
+{
+/**
+ * @brief SD reponses and error flags
+ */
+ SD_RESPONSE_NO_ERROR = (0x00),
+ SD_IN_IDLE_STATE = (0x01),
+ SD_ERASE_RESET = (0x02),
+ SD_ILLEGAL_COMMAND = (0x04),
+ SD_COM_CRC_ERROR = (0x08),
+ SD_ERASE_SEQUENCE_ERROR = (0x10),
+ SD_ADDRESS_ERROR = (0x20),
+ SD_PARAMETER_ERROR = (0x40),
+ SD_RESPONSE_FAILURE = (0xFF),
+
+/**
+ * @brief Data response error
+ */
+ SD_DATA_OK = (0x05),
+ SD_DATA_CRC_ERROR = (0x0B),
+ SD_DATA_WRITE_ERROR = (0x0D),
+ SD_DATA_OTHER_ERROR = (0xFF)
+} SD_Error;
+
+/**
+ * @brief Card Specific Data: CSD Register
+ */
+typedef struct
+{
+ __IO uint8_t CSDStruct; /*!< CSD structure */
+ __IO uint8_t SysSpecVersion; /*!< System specification version */
+ __IO uint8_t Reserved1; /*!< Reserved */
+ __IO uint8_t TAAC; /*!< Data read access-time 1 */
+ __IO uint8_t NSAC; /*!< Data read access-time 2 in CLK cycles */
+ __IO uint8_t MaxBusClkFrec; /*!< Max. bus clock frequency */
+ __IO uint16_t CardComdClasses; /*!< Card command classes */
+ __IO uint8_t RdBlockLen; /*!< Max. read data block length */
+ __IO uint8_t PartBlockRead; /*!< Partial blocks for read allowed */
+ __IO uint8_t WrBlockMisalign; /*!< Write block misalignment */
+ __IO uint8_t RdBlockMisalign; /*!< Read block misalignment */
+ __IO uint8_t DSRImpl; /*!< DSR implemented */
+ __IO uint8_t Reserved2; /*!< Reserved */
+ __IO uint32_t DeviceSize; /*!< Device Size */
+ __IO uint8_t MaxRdCurrentVDDMin; /*!< Max. read current @ VDD min */
+ __IO uint8_t MaxRdCurrentVDDMax; /*!< Max. read current @ VDD max */
+ __IO uint8_t MaxWrCurrentVDDMin; /*!< Max. write current @ VDD min */
+ __IO uint8_t MaxWrCurrentVDDMax; /*!< Max. write current @ VDD max */
+ __IO uint8_t DeviceSizeMul; /*!< Device size multiplier */
+ __IO uint8_t EraseGrSize; /*!< Erase group size */
+ __IO uint8_t EraseGrMul; /*!< Erase group size multiplier */
+ __IO uint8_t WrProtectGrSize; /*!< Write protect group size */
+ __IO uint8_t WrProtectGrEnable; /*!< Write protect group enable */
+ __IO uint8_t ManDeflECC; /*!< Manufacturer default ECC */
+ __IO uint8_t WrSpeedFact; /*!< Write speed factor */
+ __IO uint8_t MaxWrBlockLen; /*!< Max. write data block length */
+ __IO uint8_t WriteBlockPaPartial; /*!< Partial blocks for write allowed */
+ __IO uint8_t Reserved3; /*!< Reserded */
+ __IO uint8_t ContentProtectAppli; /*!< Content protection application */
+ __IO uint8_t FileFormatGrouop; /*!< File format group */
+ __IO uint8_t CopyFlag; /*!< Copy flag (OTP) */
+ __IO uint8_t PermWrProtect; /*!< Permanent write protection */
+ __IO uint8_t TempWrProtect; /*!< Temporary write protection */
+ __IO uint8_t FileFormat; /*!< File Format */
+ __IO uint8_t ECC; /*!< ECC code */
+ __IO uint8_t CSD_CRC; /*!< CSD CRC */
+ __IO uint8_t Reserved4; /*!< always 1*/
+} SD_CSD;
+
+/**
+ * @brief Card Identification Data: CID Register
+ */
+typedef struct
+{
+ __IO uint8_t ManufacturerID; /*!< ManufacturerID */
+ __IO uint16_t OEM_AppliID; /*!< OEM/Application ID */
+ __IO uint32_t ProdName1; /*!< Product Name part1 */
+ __IO uint8_t ProdName2; /*!< Product Name part2*/
+ __IO uint8_t ProdRev; /*!< Product Revision */
+ __IO uint32_t ProdSN; /*!< Product Serial Number */
+ __IO uint8_t Reserved1; /*!< Reserved1 */
+ __IO uint16_t ManufactDate; /*!< Manufacturing Date */
+ __IO uint8_t CID_CRC; /*!< CID CRC */
+ __IO uint8_t Reserved2; /*!< always 1 */
+} SD_CID;
+
+/**
+ * @brief SD Card information
+ */
+typedef struct
+{
+ SD_CSD SD_csd;
+ SD_CID SD_cid;
+ uint32_t CardCapacity; /*!< Card Capacity */
+ uint32_t CardBlockSize; /*!< Card Block Size */
+} SD_CardInfo;
+
+
+/** @defgroup STM32303C_EVAL_SPI_SD_Exported_Constants
+ * @{
+ */
+
+/**
+ * @brief Block Size
+ */
+#define SD_BLOCK_SIZE 0x200
+
+/**
+ * @brief Dummy byte
+ */
+#define SD_DUMMY_BYTE 0xFF
+
+/**
+ * @brief Start Data tokens:
+ * Tokens (necessary because at nop/idle (and CS active) only 0xff is
+ * on the data/command line)
+ */
+#define SD_START_DATA_SINGLE_BLOCK_READ 0xFE /*!< Data token start byte, Start Single Block Read */
+#define SD_START_DATA_MULTIPLE_BLOCK_READ 0xFE /*!< Data token start byte, Start Multiple Block Read */
+#define SD_START_DATA_SINGLE_BLOCK_WRITE 0xFE /*!< Data token start byte, Start Single Block Write */
+#define SD_START_DATA_MULTIPLE_BLOCK_WRITE 0xFD /*!< Data token start byte, Start Multiple Block Write */
+#define SD_STOP_DATA_MULTIPLE_BLOCK_WRITE 0xFD /*!< Data toke stop byte, Stop Multiple Block Write */
+
+/**
+ * @brief SD detection on its memory slot
+ */
+#define SD_PRESENT ((uint8_t)0x01)
+#define SD_NOT_PRESENT ((uint8_t)0x00)
+
+
+/**
+ * @brief Commands: CMDxx = CMD-number | 0x40
+ */
+#define SD_CMD_GO_IDLE_STATE 0 /*!< CMD0 = 0x40 */
+#define SD_CMD_SEND_OP_COND 1 /*!< CMD1 = 0x41 */
+#define SD_CMD_SEND_IF_COND 8
+#define SD_CMD_SEND_CSD 9 /*!< CMD9 = 0x49 */
+#define SD_CMD_SEND_CID 10 /*!< CMD10 = 0x4A */
+#define SD_CMD_STOP_TRANSMISSION 12 /*!< CMD12 = 0x4C */
+#define SD_CMD_SEND_STATUS 13 /*!< CMD13 = 0x4D */
+#define SD_CMD_SET_BLOCKLEN 16 /*!< CMD16 = 0x50 */
+#define SD_CMD_READ_SINGLE_BLOCK 17 /*!< CMD17 = 0x51 */
+#define SD_CMD_READ_MULT_BLOCK 18 /*!< CMD18 = 0x52 */
+#define SD_CMD_SET_BLOCK_COUNT 23 /*!< CMD23 = 0x57 */
+#define SD_CMD_WRITE_SINGLE_BLOCK 24 /*!< CMD24 = 0x58 */
+#define SD_CMD_WRITE_MULT_BLOCK 25 /*!< CMD25 = 0x59 */
+#define SD_CMD_PROG_CSD 27 /*!< CMD27 = 0x5B */
+#define SD_CMD_SET_WRITE_PROT 28 /*!< CMD28 = 0x5C */
+#define SD_CMD_CLR_WRITE_PROT 29 /*!< CMD29 = 0x5D */
+#define SD_CMD_SEND_WRITE_PROT 30 /*!< CMD30 = 0x5E */
+#define SD_CMD_SD_ERASE_GRP_START 32 /*!< CMD32 = 0x60 */
+#define SD_CMD_SD_ERASE_GRP_END 33 /*!< CMD33 = 0x61 */
+#define SD_CMD_UNTAG_SECTOR 34 /*!< CMD34 = 0x62 */
+#define SD_CMD_ERASE_GRP_START 35 /*!< CMD35 = 0x63 */
+#define SD_CMD_ERASE_GRP_END 36 /*!< CMD36 = 0x64 */
+#define SD_CMD_UNTAG_ERASE_GROUP 37 /*!< CMD37 = 0x65 */
+#define SD_CMD_ERASE 38 /*!< CMD38 = 0x66 */
+
+/**
+ * @}
+ */
+
+/** @defgroup STM32303C_EVAL_SPI_SD_Exported_Macros
+ * @{
+ */
+/**
+ * @brief Select SD Card: ChipSelect pin low
+ */
+#define SD_CS_LOW() GPIO_ResetBits(SD_CS_GPIO, SD_CS_PIN)
+/**
+ * @brief Deselect SD Card: ChipSelect pin high
+ */
+#define SD_CS_HIGH() GPIO_SetBits(SD_CS_GPIO, SD_CS_PIN)
+/**
+ * @}
+ */
+
+/** @defgroup STM32303C_EVAL_SPI_SD_Exported_Functions
+ * @{
+ */
+void SD_DeInit(void);
+SD_Error SD_Init(void);
+uint8_t SD_Detect(void);
+SD_Error SD_GetCardInfo(SD_CardInfo *cardinfo);
+SD_Error SD_ReadBlock(uint8_t* pBuffer, uint32_t ReadAddr, uint16_t BlockSize);
+SD_Error SD_ReadMultiBlocks(uint8_t* pBuffer, uint32_t ReadAddr, uint16_t BlockSize, uint32_t NumberOfBlocks);
+SD_Error SD_WriteBlock(uint8_t* pBuffer, uint32_t WriteAddr, uint16_t BlockSize);
+SD_Error SD_WriteMultiBlocks(uint8_t* pBuffer, uint32_t WriteAddr, uint16_t BlockSize, uint32_t NumberOfBlocks);
+SD_Error SD_GetCSDRegister(SD_CSD* SD_csd);
+SD_Error SD_GetCIDRegister(SD_CID* SD_cid);
+
+void SD_SendCmd(uint8_t Cmd, uint32_t Arg, uint8_t Crc);
+SD_Error SD_GetResponse(uint8_t Response);
+uint8_t SD_GetDataResponse(void);
+SD_Error SD_GoIdleState(void);
+uint16_t SD_GetStatus(void);
+
+uint8_t SD_WriteByte(uint8_t byte);
+uint8_t SD_ReadByte(void);
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index d73b2d45f7..28464ec5f0 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -45,6 +45,7 @@
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
+#include "drivers/sdcard.h"
#include "drivers/buf_writer.h"
@@ -155,6 +156,15 @@ static void cliFlashRead(char *cmdline);
#endif
#endif
+#ifdef USE_SERIAL_1WIRE
+static void cliUSB1Wire(char *cmdline);
+#endif
+
+#ifdef USE_SD_CARD
+static void cliSdInfo(char *cmdline);
+static void cliSdRead(char *cmdline);
+#endif
+
// buffer
static char cliBuffer[48];
static uint32_t bufferIndex = 0;
@@ -291,6 +301,10 @@ const clicmd_t cmdTable[] = {
"\treset\r\n"
"\tload \r\n"
"\treverse r|n", cliServoMix),
+#endif
+#ifdef USE_SD_CARD
+ CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo),
+ CLI_COMMAND_DEF("sd_read", NULL, " ", cliSdRead),
#endif
CLI_COMMAND_DEF("status", "show status", NULL, cliStatus),
#ifndef SKIP_TASK_STATISTICS
@@ -1482,6 +1496,103 @@ static void cliServoMix(char *cmdline)
}
#endif
+#ifdef USE_SD_CARD
+
+static void cliSdShowError(char *message, SD_Error error)
+{
+ cliPrintf("%s: %d\r\n", message, error);
+}
+
+static void cliSdInfo(char *cmdline) {
+ UNUSED(cmdline);
+
+ SD_Error error;
+
+ uint8_t sdPresent = SD_Detect();
+ cliPrintf("sdcard %s\n", sdPresent == SD_PRESENT ? "PRESENT" : "NOT PRESENT");
+
+ if (sdPresent == SD_PRESENT) {
+ error = SD_Init();
+ cliSdShowError("SD init result", error);
+ }
+
+ SD_CardInfo cardInfo;
+ memset(&cardInfo, 0, sizeof(cardInfo));
+ error = SD_GetCardInfo(&cardInfo);
+
+ cliSdShowError("SD card info result", error);
+ if (error == SD_RESPONSE_NO_ERROR) {
+ cliPrintf("capacity: %d, block size: %d\r\n",
+ cardInfo.CardCapacity,
+ cardInfo.CardBlockSize
+ );
+ }
+ SD_DeInit();
+}
+
+static void cliSdRead(char *cmdline)
+{
+ uint32_t address = atoi(cmdline);
+ uint32_t length;
+ int i;
+
+ uint8_t buffer[32];
+
+ uint8_t sdPresent = SD_Detect();
+ if (sdPresent != SD_PRESENT) {
+ cliPrint("sd card not present\r\n");
+ return;
+ }
+
+ SD_Error error;
+
+ error = SD_Init();
+ if (error) {
+ SD_DeInit();
+ return;
+ }
+
+ char *nextArg = strchr(cmdline, ' ');
+
+ if (!nextArg) {
+ cliShowParseError();
+ } else {
+ length = atoi(nextArg);
+
+ cliPrintf("Reading %u bytes at %u:\r\n", length, address);
+
+ while (length > 0) {
+ int bytesRead;
+ int bytesToRead = length < sizeof(buffer) ? length : sizeof(buffer);
+
+ error = SD_ReadBlock(buffer, address, bytesToRead);
+
+ if (error != SD_RESPONSE_NO_ERROR) {
+ cliSdShowError("SD init result", error);
+ break;
+ }
+
+ bytesRead = bytesToRead;
+
+ for (i = 0; i < bytesRead; i++) {
+ cliWrite(buffer[i]);
+ }
+
+ length -= bytesRead;
+ address += bytesRead;
+
+ if (bytesRead == 0) {
+ //Assume we reached the end of the volume or something fatal happened
+ break;
+ }
+ }
+ cliPrintf("\r\n");
+ }
+
+ SD_DeInit();
+}
+
+#endif
#ifdef USE_FLASHFS
diff --git a/src/main/main.c b/src/main/main.c
index 8e4cc1f6a9..37a4a113ca 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -49,6 +49,7 @@
#include "drivers/bus_spi.h"
#include "drivers/inverter.h"
#include "drivers/flash_m25p16.h"
+#include "drivers/sdcard.h"
#include "drivers/sonar_hcsr04.h"
#include "drivers/gyro_sync.h"
@@ -343,6 +344,19 @@ void init(void)
#endif
#endif
+ SD_Error error;
+
+ SD_Detect_LowLevel_Init();
+ if (SD_Detect() == SD_PRESENT) {
+ while ((error = SD_Init()) != SD_RESPONSE_NO_ERROR) {
+
+ };
+
+ SD_DeInit();
+ }
+ SD_Detect_LowLevel_DeInit();
+
+
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
@@ -477,6 +491,10 @@ void init(void)
}
#endif
+#ifdef USE_SD_CARD
+ SD_Detect_LowLevel_Init();
+#endif
+
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h
new file mode 100644
index 0000000000..088f18b647
--- /dev/null
+++ b/src/main/target/CHEBUZZF3/target.h
@@ -0,0 +1,159 @@
+/*
+ * 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
+
+#define TARGET_BOARD_IDENTIFIER "CHF3" // Chebuzz F3
+
+#define LED0_GPIO GPIOE
+#define LED0_PIN Pin_8|Pin_12 // Blue LEDs - PE8/PE12
+#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOE
+#define LED0_INVERTED
+#define LED1_GPIO GPIOE
+#define LED1_PIN Pin_10|Pin_14 // Orange LEDs - PE10/PE14
+#define LED1_PERIPHERAL RCC_AHBPeriph_GPIOE
+#define LED1_INVERTED
+
+#define BEEP_GPIO GPIOE
+#define BEEP_PIN Pin_9|Pin_13 // Red LEDs - PE9/PE13
+#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOE
+#define BEEPER_INVERTED
+
+#define USABLE_TIMER_CHANNEL_COUNT 18
+
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+#define USE_SPI_DEVICE_2
+
+#define SPI2_GPIO GPIOB
+#define SPI2_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB
+#define SPI2_NSS_PIN Pin_12
+#define SPI2_NSS_PIN_SOURCE GPIO_PinSource12
+#define SPI2_SCK_PIN Pin_13
+#define SPI2_SCK_PIN_SOURCE GPIO_PinSource13
+#define SPI2_MISO_PIN Pin_14
+#define SPI2_MISO_PIN_SOURCE GPIO_PinSource14
+#define SPI2_MOSI_PIN Pin_15
+#define SPI2_MOSI_PIN_SOURCE GPIO_PinSource15
+
+#define USE_SD_CARD
+
+#define SD_DETECT_PIN GPIO_Pin_14
+#define SD_DETECT_EXTI_LINE EXTI_Line14
+#define SD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource14
+#define SD_DETECT_GPIO_PORT GPIOC
+#define SD_DETECT_GPIO_CLK RCC_AHBPeriph_GPIOC
+#define SD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOC
+#define SD_DETECT_EXTI_IRQn EXTI15_10_IRQn
+
+#define SD_CS_GPIO GPIOB
+#define SD_CS_PIN GPIO_Pin_12
+#define SD_SPI_INSTANCE SPI2
+
+//#define USE_FLASHFS
+//#define USE_FLASH_M25P16
+
+//#define M25P16_CS_GPIO GPIOB
+//#define M25P16_CS_PIN GPIO_Pin_12
+//#define M25P16_SPI_INSTANCE SPI2
+
+#define GYRO
+#define USE_GYRO_L3GD20
+#define USE_GYRO_MPU6050
+
+#define L3GD20_SPI SPI1
+#define L3GD20_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOE
+#define L3GD20_CS_GPIO GPIOE
+#define L3GD20_CS_PIN GPIO_Pin_3
+
+#define GYRO_L3GD20_ALIGN CW270_DEG
+#define GYRO_MPU6050_ALIGN CW0_DEG
+
+#define ACC
+#define USE_ACC_MPU6050
+#define USE_ACC_LSM303DLHC
+
+#define ACC_MPU6050_ALIGN CW0_DEG
+
+#define BARO
+#define USE_BARO_MS5611
+
+#define MAG
+#define USE_MAG_AK8975
+
+#define MAG_AK8975_ALIGN CW90_DEG_FLIP
+
+#define BEEPER
+#define LED0
+#define LED1
+
+#define USE_VCP
+#define USE_USART1
+#define USE_USART2
+#define SERIAL_PORT_COUNT 3
+
+#define USE_I2C
+#define I2C_DEVICE (I2CDEV_1)
+
+#define USE_ADC
+
+#define ADC_INSTANCE ADC1
+#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1
+#define ADC_DMA_CHANNEL DMA1_Channel1
+
+#define VBAT_ADC_GPIO GPIOC
+#define VBAT_ADC_GPIO_PIN GPIO_Pin_0
+#define VBAT_ADC_CHANNEL ADC_Channel_6
+
+#define CURRENT_METER_ADC_GPIO GPIOC
+#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1
+#define CURRENT_METER_ADC_CHANNEL ADC_Channel_7
+
+#define RSSI_ADC_GPIO GPIOC
+#define RSSI_ADC_GPIO_PIN GPIO_Pin_2
+#define RSSI_ADC_CHANNEL ADC_Channel_8
+
+#define EXTERNAL1_ADC_GPIO GPIOC
+#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3
+#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9
+
+#define GPS
+#define LED_STRIP
+#if 1
+#define LED_STRIP_TIMER TIM16
+#else
+// alternative LED strip configuration, tested working.
+#define LED_STRIP_TIMER TIM1
+
+#define USE_LED_STRIP_ON_DMA1_CHANNEL2
+#define WS2811_GPIO GPIOA
+#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA
+#define WS2811_GPIO_AF GPIO_AF_6
+#define WS2811_PIN GPIO_Pin_8
+#define WS2811_PIN_SOURCE GPIO_PinSource8
+#define WS2811_TIMER TIM1
+#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1
+#define WS2811_DMA_CHANNEL DMA1_Channel2
+#define WS2811_IRQ DMA1_Channel2_IRQn
+#endif
+
+#define BLACKBOX
+#define GTUNE
+#define TELEMETRY
+#define SERIAL_RX
+#define USE_SERVOS
+#define USE_CLI
diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h
new file mode 100644
index 0000000000..ec5ea80565
--- /dev/null
+++ b/src/main/target/STM32F3DISCOVERY/target.h
@@ -0,0 +1,143 @@
+/*
+ * 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
+
+#define TARGET_BOARD_IDENTIFIER "SDF3" // STM Discovery F3
+
+#define LED0_GPIO GPIOE
+#define LED0_PIN Pin_8|Pin_12 // Blue LEDs - PE8/PE12
+#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOE
+#define LED0_INVERTED
+#define LED1_GPIO GPIOE
+#define LED1_PIN Pin_10|Pin_14 // Orange LEDs - PE10/PE14
+#define LED1_PERIPHERAL RCC_AHBPeriph_GPIOE
+#define LED1_INVERTED
+
+#define BEEP_GPIO GPIOE
+#define BEEP_PIN Pin_9|Pin_13 // Red LEDs - PE9/PE13
+#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOE
+#define BEEPER_INVERTED
+
+
+#define BEEPER_INVERTED
+
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+#define USE_SPI_DEVICE_2
+
+#define SPI2_GPIO GPIOB
+#define SPI2_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB
+#define SPI2_NSS_PIN Pin_12
+#define SPI2_NSS_PIN_SOURCE GPIO_PinSource12
+#define SPI2_SCK_PIN Pin_13
+#define SPI2_SCK_PIN_SOURCE GPIO_PinSource13
+#define SPI2_MISO_PIN Pin_14
+#define SPI2_MISO_PIN_SOURCE GPIO_PinSource14
+#define SPI2_MOSI_PIN Pin_15
+#define SPI2_MOSI_PIN_SOURCE GPIO_PinSource15
+
+#define USE_SD_CARD
+
+#define SD_DETECT_PIN GPIO_Pin_14
+#define SD_DETECT_EXTI_LINE EXTI_Line14
+#define SD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource14
+#define SD_DETECT_GPIO_PORT GPIOC
+#define SD_DETECT_GPIO_CLK RCC_AHBPeriph_GPIOC
+#define SD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOC
+#define SD_DETECT_EXTI_IRQn EXTI15_10_IRQn
+
+#define SD_CS_GPIO GPIOB
+#define SD_CS_PIN GPIO_Pin_12
+#define SD_SPI_INSTANCE SPI2
+
+//#define USE_FLASHFS
+//#define USE_FLASH_M25P16
+
+//#define M25P16_CS_GPIO GPIOB
+//#define M25P16_CS_PIN GPIO_Pin_12
+//#define M25P16_SPI_INSTANCE SPI2
+
+#define GYRO
+#define USE_GYRO_L3GD20
+
+#define L3GD20_SPI SPI1
+#define L3GD20_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOE
+#define L3GD20_CS_GPIO GPIOE
+#define L3GD20_CS_PIN GPIO_Pin_3
+
+#define GYRO_L3GD20_ALIGN CW270_DEG
+
+#define ACC
+#define USE_ACC_LSM303DLHC
+
+#define MAG
+#define USE_MAG_HMC5883
+
+#define BEEPER
+#define LED0
+#define LED1
+
+#define USE_VCP
+#define USE_USART1
+#define USE_USART2
+#define SERIAL_PORT_COUNT 3
+
+#define USE_I2C
+#define I2C_DEVICE (I2CDEV_1)
+
+#define USE_ADC
+
+#define ADC_INSTANCE ADC1
+#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1
+#define ADC_DMA_CHANNEL DMA1_Channel1
+
+#define VBAT_ADC_GPIO GPIOC
+#define VBAT_ADC_GPIO_PIN GPIO_Pin_0
+#define VBAT_ADC_CHANNEL ADC_Channel_6
+
+#define CURRENT_METER_ADC_GPIO GPIOC
+#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1
+#define CURRENT_METER_ADC_CHANNEL ADC_Channel_7
+
+#define RSSI_ADC_GPIO GPIOC
+#define RSSI_ADC_GPIO_PIN GPIO_Pin_2
+#define RSSI_ADC_CHANNEL ADC_Channel_8
+
+#define EXTERNAL1_ADC_GPIO GPIOC
+#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3
+#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9
+
+#define BLACKBOX
+#define GPS
+#define GTUNE
+#define LED_STRIP
+#define LED_STRIP_TIMER TIM16
+#define TELEMETRY
+#define SERIAL_RX
+#define USE_SERVOS
+#define USE_CLI
+
+#define USE_SERIAL_1WIRE
+// How many escs does this board support?
+#define ESC_COUNT 6
+// STM32F3DISCOVERY TX - PD5 connects to UART RX
+#define S1W_TX_GPIO GPIOD
+#define S1W_TX_PIN GPIO_Pin_5
+// STM32F3DISCOVERY RX - PD6 connects to UART TX
+#define S1W_RX_GPIO GPIOD
+#define S1W_RX_PIN GPIO_Pin_6