From 4b955f09da31d01795e57d367e965fe1e7c5a507 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 26 Aug 2015 01:26:26 +0100 Subject: [PATCH] SDCard - Basic functionality. Detect card, query capacity, read data. --- Makefile | 1 + src/main/drivers/sdcard.c | 826 ++++++++++++++++++++++ src/main/drivers/sdcard.h | 223 ++++++ src/main/io/serial_cli.c | 111 +++ src/main/main.c | 18 + src/main/target/CHEBUZZF3/target.h | 159 +++++ src/main/target/STM32F3DISCOVERY/target.h | 143 ++++ 7 files changed, 1481 insertions(+) create mode 100644 src/main/drivers/sdcard.c create mode 100644 src/main/drivers/sdcard.h create mode 100644 src/main/target/CHEBUZZF3/target.h create mode 100644 src/main/target/STM32F3DISCOVERY/target.h 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