diff --git a/src/main/drivers/bus_bst.h b/src/main/drivers/bus_bst.h index 23ba48d2f9..3ade4baeec 100644 --- a/src/main/drivers/bus_bst.h +++ b/src/main/drivers/bus_bst.h @@ -32,6 +32,8 @@ #define CROSSFIRE_RSSI_FRAME_ID 0x14 #define CLEANFLIGHT_MODE_FRAME_ID 0x20 +#define DATA_BUFFER_SIZE 64 + typedef enum BSTDevice { BSTDEV_1, BSTDEV_2, @@ -39,6 +41,7 @@ typedef enum BSTDevice { } BSTDevice; void bstInit(BSTDevice index); +uint32_t bstTimeoutUserCallback(void); uint16_t bstGetErrorCounter(void); bool bstWriteBusy(void); @@ -47,7 +50,6 @@ bool bstSlaveRead(uint8_t* buf); bool bstSlaveWrite(uint8_t* data); void bstMasterWriteLoop(void); -void bstMasterReadLoop(void); void crc8Cal(uint8_t data_in); diff --git a/src/main/drivers/bus_bst_stm32f30x.c b/src/main/drivers/bus_bst_stm32f30x.c index d63323445f..201985e9e0 100644 --- a/src/main/drivers/bus_bst_stm32f30x.c +++ b/src/main/drivers/bus_bst_stm32f30x.c @@ -12,6 +12,7 @@ #include +#include "nvic.h" #include "bus_bst.h" @@ -45,8 +46,6 @@ #define BST2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA #endif -static uint32_t bstTimeout; - static volatile uint16_t bst1ErrorCount = 0; static volatile uint16_t bst2ErrorCount = 0; @@ -59,101 +58,219 @@ volatile bool coreProReady = false; // BST TimeoutUserCallback /////////////////////////////////////////////////////////////////////////////// -uint32_t bstTimeoutUserCallback(I2C_TypeDef *BSTx) +uint8_t dataBuffer[DATA_BUFFER_SIZE] = {0}; +uint8_t dataBufferPointer = 0; +uint8_t bstWriteDataLen = 0; + +uint32_t micros(void); + +uint8_t writeData[DATA_BUFFER_SIZE] = {0}; +uint8_t currentWriteBufferPointer = 0; +bool receiverAddress = false; + +uint8_t readData[DATA_BUFFER_SIZE] = {0}; +uint8_t bufferPointer = 0; + +void bstProcessInCommand(void); +void I2C_EV_IRQHandler() { - if (BSTx == I2C1) { - bst1ErrorCount++; - } else { - bst2ErrorCount++; - } - I2C_SoftwareResetCmd(BSTx); - return false; + if(I2C_GetITStatus(BSTx, I2C_IT_ADDR)) { + CRC8 = 0; + if(I2C_GetTransferDirection(BSTx) == I2C_Direction_Receiver) { + currentWriteBufferPointer = 0; + receiverAddress = true; + I2C_SendData(BSTx, (uint8_t) writeData[currentWriteBufferPointer++]); + I2C_ITConfig(BSTx, I2C_IT_TXI, ENABLE); + } else { + readData[0] = I2C_GetAddressMatched(BSTx); + bufferPointer = 1; + } + I2C_ClearITPendingBit(BSTx, I2C_IT_ADDR); + } else if(I2C_GetITStatus(BSTx, I2C_IT_RXNE)) { + uint8_t data = I2C_ReceiveData(BSTx); + readData[bufferPointer] = data; + if(bufferPointer > 1) { + if(readData[1]+1 == bufferPointer) { + crc8Cal(0); + bstProcessInCommand(); + } else { + crc8Cal(data); + } + } + bufferPointer++; + I2C_ClearITPendingBit(BSTx, I2C_IT_RXNE); + } else if(I2C_GetITStatus(BSTx, I2C_IT_TXIS)) { + if(receiverAddress) { + if(currentWriteBufferPointer > 0) { + if(writeData[0] == currentWriteBufferPointer) { + receiverAddress = false; + crc8Cal(0); + I2C_SendData(BSTx, (uint8_t) CRC8); + I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE); + } else { + crc8Cal((uint8_t) writeData[currentWriteBufferPointer]); + I2C_SendData(BSTx, (uint8_t) writeData[currentWriteBufferPointer++]); + } + } + } else if(bstWriteDataLen) { + I2C_SendData(BSTx, (uint8_t) dataBuffer[dataBufferPointer]); + if(bstWriteDataLen > 1) + dataBufferPointer++; + if(dataBufferPointer == bstWriteDataLen) { + I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE); + dataBufferPointer = 0; + bstWriteDataLen = 0; + } + } else { + } + I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS); + } else if(I2C_GetITStatus(BSTx, I2C_IT_NACKF)) { + if(receiverAddress) { + receiverAddress = false; + I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE); + } + I2C_ClearITPendingBit(BSTx, I2C_IT_NACKF); + } else if(I2C_GetITStatus(BSTx, I2C_IT_STOPF)) { + if(bstWriteDataLen && dataBufferPointer == bstWriteDataLen) { + dataBufferPointer = 0; + bstWriteDataLen = 0; + } + I2C_ClearITPendingBit(BSTx, I2C_IT_STOPF); + } else if(I2C_GetITStatus(BSTx, I2C_IT_BERR) + || I2C_GetITStatus(BSTx, I2C_IT_ARLO) + || I2C_GetITStatus(BSTx, I2C_IT_OVR)) { + bstTimeoutUserCallback(); + I2C_ClearITPendingBit(BSTx, I2C_IT_BERR | I2C_IT_ARLO | I2C_IT_OVR); + } +} + +void I2C1_EV_IRQHandler() +{ + I2C_EV_IRQHandler(); +} + +void I2C2_EV_IRQHandler() +{ + I2C_EV_IRQHandler(); +} + +uint32_t bstTimeoutUserCallback() +{ + if (BSTx == I2C1) { + bst1ErrorCount++; + } else { + bst2ErrorCount++; + } + I2C_GenerateSTOP(BSTx, ENABLE); + receiverAddress = false; + dataBufferPointer = 0; + bstWriteDataLen = 0; + I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE); + I2C_SoftwareResetCmd(BSTx); + return false; } void bstInitPort(I2C_TypeDef *BSTx/*, uint8_t Address*/) { - GPIO_InitTypeDef GPIO_InitStructure; - I2C_InitTypeDef BST_InitStructure; + NVIC_InitTypeDef nvic; + GPIO_InitTypeDef GPIO_InitStructure; + I2C_InitTypeDef BST_InitStructure; - if (BSTx == I2C1) { - RCC_AHBPeriphClockCmd(BST1_SCL_CLK_SOURCE | BST1_SDA_CLK_SOURCE, ENABLE); - RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); - RCC_I2CCLKConfig(RCC_I2C1CLK_SYSCLK); + if(BSTx == I2C1) { + RCC_AHBPeriphClockCmd(BST1_SCL_CLK_SOURCE | BST1_SDA_CLK_SOURCE, ENABLE); + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); + RCC_I2CCLKConfig(RCC_I2C1CLK_SYSCLK); - GPIO_PinAFConfig(BST1_SCL_GPIO, BST1_SCL_PIN_SOURCE, BST1_SCL_GPIO_AF); - GPIO_PinAFConfig(BST1_SDA_GPIO, BST1_SDA_PIN_SOURCE, BST1_SDA_GPIO_AF); + GPIO_PinAFConfig(BST1_SCL_GPIO, BST1_SCL_PIN_SOURCE, BST1_SCL_GPIO_AF); + GPIO_PinAFConfig(BST1_SDA_GPIO, BST1_SDA_PIN_SOURCE, BST1_SDA_GPIO_AF); - GPIO_StructInit(&GPIO_InitStructure); - I2C_StructInit(&BST_InitStructure); + GPIO_StructInit(&GPIO_InitStructure); + I2C_StructInit(&BST_InitStructure); - // Init pins - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + // Init pins + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_InitStructure.GPIO_Pin = BST1_SCL_PIN; - GPIO_Init(BST1_SCL_GPIO, &GPIO_InitStructure); + GPIO_InitStructure.GPIO_Pin = BST1_SCL_PIN; + GPIO_Init(BST1_SCL_GPIO, &GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = BST1_SDA_PIN; - GPIO_Init(BST1_SDA_GPIO, &GPIO_InitStructure); + GPIO_InitStructure.GPIO_Pin = BST1_SDA_PIN; + GPIO_Init(BST1_SDA_GPIO, &GPIO_InitStructure); - I2C_StructInit(&BST_InitStructure); + I2C_StructInit(&BST_InitStructure); - BST_InitStructure.I2C_Mode = I2C_Mode_I2C; - BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable; - BST_InitStructure.I2C_DigitalFilter = 0x00; - BST_InitStructure.I2C_OwnAddress1 = CLEANFLIGHT_FC; - //BST_InitStructure.I2C_OwnAddress1 = PNP_PRO_GPS; - //BST_InitStructure.I2C_OwnAddress1 = Address; - BST_InitStructure.I2C_Ack = I2C_Ack_Enable; - BST_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; - BST_InitStructure.I2C_Timing = 0x30E0257A; // 100 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. + BST_InitStructure.I2C_Mode = I2C_Mode_I2C; + BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable; + BST_InitStructure.I2C_DigitalFilter = 0x00; + BST_InitStructure.I2C_OwnAddress1 = CLEANFLIGHT_FC; + BST_InitStructure.I2C_Ack = I2C_Ack_Enable; + BST_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; + BST_InitStructure.I2C_Timing = 0x30E0257A; // 100 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. - I2C_Init(I2C1, &BST_InitStructure); + I2C_Init(I2C1, &BST_InitStructure); - I2C_Cmd(I2C1, ENABLE); - } + I2C_GeneralCallCmd(I2C1, ENABLE); - if (BSTx == I2C2) { - RCC_AHBPeriphClockCmd(BST2_SCL_CLK_SOURCE | BST2_SDA_CLK_SOURCE, ENABLE); - RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); - RCC_I2CCLKConfig(RCC_I2C2CLK_SYSCLK); + nvic.NVIC_IRQChannel = I2C1_EV_IRQn; + nvic.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_BST_READ_DATA); + nvic.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_BST_READ_DATA); + nvic.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&nvic); - GPIO_PinAFConfig(BST2_SCL_GPIO, BST2_SCL_PIN_SOURCE, BST2_SCL_GPIO_AF); - GPIO_PinAFConfig(BST2_SDA_GPIO, BST2_SDA_PIN_SOURCE, BST2_SDA_GPIO_AF); + I2C_ITConfig(I2C1, I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ERRI, ENABLE); + + I2C_Cmd(I2C1, ENABLE); + } - GPIO_StructInit(&GPIO_InitStructure); - I2C_StructInit(&BST_InitStructure); + if(BSTx == I2C2) { + RCC_AHBPeriphClockCmd(BST2_SCL_CLK_SOURCE | BST2_SDA_CLK_SOURCE, ENABLE); + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); + RCC_I2CCLKConfig(RCC_I2C2CLK_SYSCLK); - // Init pins - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_PinAFConfig(BST2_SCL_GPIO, BST2_SCL_PIN_SOURCE, BST2_SCL_GPIO_AF); + GPIO_PinAFConfig(BST2_SDA_GPIO, BST2_SDA_PIN_SOURCE, BST2_SDA_GPIO_AF); - GPIO_InitStructure.GPIO_Pin = BST2_SCL_PIN; - GPIO_Init(BST2_SCL_GPIO, &GPIO_InitStructure); + GPIO_StructInit(&GPIO_InitStructure); + I2C_StructInit(&BST_InitStructure); - GPIO_InitStructure.GPIO_Pin = BST2_SDA_PIN; - GPIO_Init(BST2_SDA_GPIO, &GPIO_InitStructure); + // Init pins + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - I2C_StructInit(&BST_InitStructure); + GPIO_InitStructure.GPIO_Pin = BST2_SCL_PIN; + GPIO_Init(BST2_SCL_GPIO, &GPIO_InitStructure); - BST_InitStructure.I2C_Mode = I2C_Mode_I2C; - BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable; - BST_InitStructure.I2C_DigitalFilter = 0x00; - BST_InitStructure.I2C_OwnAddress1 = CLEANFLIGHT_FC; - //BST_InitStructure.I2C_OwnAddress1 = PNP_PRO_GPS; - //BST_InitStructure.I2C_OwnAddress1 = Address; - BST_InitStructure.I2C_Ack = I2C_Ack_Enable; - BST_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; - BST_InitStructure.I2C_Timing = 0x30E0257A; // 100 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. + GPIO_InitStructure.GPIO_Pin = BST2_SDA_PIN; + GPIO_Init(BST2_SDA_GPIO, &GPIO_InitStructure); - I2C_Init(I2C2, &BST_InitStructure); + I2C_StructInit(&BST_InitStructure); - I2C_Cmd(I2C2, ENABLE); - } + BST_InitStructure.I2C_Mode = I2C_Mode_I2C; + BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable; + BST_InitStructure.I2C_DigitalFilter = 0x00; + BST_InitStructure.I2C_OwnAddress1 = CLEANFLIGHT_FC; + BST_InitStructure.I2C_Ack = I2C_Ack_Enable; + BST_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; + BST_InitStructure.I2C_Timing = 0x30E0257A; // 100 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. + + I2C_Init(I2C2, &BST_InitStructure); + + I2C_GeneralCallCmd(I2C2, ENABLE); + + nvic.NVIC_IRQChannel = I2C2_EV_IRQn; + nvic.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_BST_READ_DATA); + nvic.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_BST_READ_DATA); + nvic.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&nvic); + + I2C_ITConfig(I2C2, I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ERRI, ENABLE); + + I2C_Cmd(I2C2, ENABLE); + } } void bstInit(BSTDevice index) @@ -163,8 +280,6 @@ void bstInit(BSTDevice index) } else { BSTx = I2C2; } - //bstInitPort(BSTx, PNP_PRO_GPS); - //bstInitPort(BSTx, CLEANFLIGHT_FC); bstInitPort(BSTx); } @@ -179,9 +294,6 @@ uint16_t bstGetErrorCounter(void) } /*************************************************************************************************/ -uint8_t dataBuffer[64] = {0}; -uint8_t bufferPointer = 0; -uint8_t bstWriteDataLen = 0; bool bstWriteBusy(void) { @@ -195,7 +307,7 @@ bool bstMasterWrite(uint8_t* data) { if(bstWriteDataLen==0) { CRC8 = 0; - bufferPointer = 0; + dataBufferPointer = 0; dataBuffer[0] = *data; dataBuffer[1] = *(data+1); bstWriteDataLen = dataBuffer[1] + 2; @@ -213,163 +325,27 @@ bool bstMasterWrite(uint8_t* data) return false; } -bool bstSlaveRead(uint8_t* buf) { - if(I2C_GetAddressMatched(BSTx)==CLEANFLIGHT_FC && I2C_GetTransferDirection(BSTx) == I2C_Direction_Transmitter) { - //if(I2C_GetTransferDirection(BSTx) == I2C_Direction_Transmitter) { - uint8_t len = 0; - CRC8 = 0; - I2C_ClearFlag(BSTx, I2C_FLAG_ADDR); - - /* Wait until RXNE flag is set */ - bstTimeout = BST_LONG_TIMEOUT; - while (I2C_GetFlagStatus(BSTx, I2C_ISR_RXNE) == RESET) { - if ((bstTimeout--) == 0) { - return bstTimeoutUserCallback(BSTx); - } - } - len = I2C_ReceiveData(BSTx); - *buf = len; - buf++; - while (len) { - /* Wait until RXNE flag is set */ - bstTimeout = BST_LONG_TIMEOUT; - while (I2C_GetFlagStatus(BSTx, I2C_ISR_RXNE) == RESET) { - if ((bstTimeout--) == 0) { - return bstTimeoutUserCallback(BSTx); - } - } - /* Read data from RXDR */ - *buf = I2C_ReceiveData(BSTx); - if(len == 1) - crc8Cal(0); - else - crc8Cal((uint8_t)*buf); - - /* Point to the next location where the byte read will be saved */ - buf++; - - /* Decrement the read bytes counter */ - len--; - } - /* If all operations OK */ - return true; - - } - return false; -} - -bool bstSlaveWrite(uint8_t* data) { - bstTimeout = BST_LONG_TIMEOUT; - while (I2C_GetFlagStatus(BSTx, I2C_ISR_ADDR) == RESET) { - if ((bstTimeout--) == 0) { - return bstTimeoutUserCallback(BSTx); - } - } - if(I2C_GetAddressMatched(BSTx)==CLEANFLIGHT_FC && I2C_GetTransferDirection(BSTx) == I2C_Direction_Receiver) { - //if(I2C_GetTransferDirection(BSTx) == I2C_Direction_Receiver) { - uint8_t len = 0; - CRC8 = 0; - I2C_ClearFlag(BSTx, I2C_FLAG_ADDR); - - /* Wait until TXIS flag is set */ - bstTimeout = BST_LONG_TIMEOUT; - while (I2C_GetFlagStatus(BSTx, I2C_ISR_TXIS) == RESET) { - if ((bstTimeout--) == 0) { - return bstTimeoutUserCallback(BSTx); - } - } - len = *data; - data++; - I2C_SendData(BSTx, (uint8_t) len); - while(len) { - /* Wait until TXIS flag is set */ - bstTimeout = BST_LONG_TIMEOUT; - while (I2C_GetFlagStatus(BSTx, I2C_ISR_TXIS) == RESET) { - if ((bstTimeout--) == 0) { - return bstTimeoutUserCallback(BSTx); - } - } - if(len == 1) { - crc8Cal(0); - I2C_SendData(BSTx, (uint8_t) CRC8); - } else { - crc8Cal((uint8_t)*data); - I2C_SendData(BSTx, (uint8_t)*data); - } - /* Point to the next location where the byte read will be saved */ - data++; - - /* Decrement the read bytes counter */ - len--; - } - /* Wait until TXIS flag is set */ - bstTimeout = BST_LONG_TIMEOUT; - while (I2C_GetFlagStatus(BSTx, I2C_ISR_TXIS) == RESET) { - if ((bstTimeout--) == 0) { - return bstTimeoutUserCallback(BSTx); - } - } - /* If all operations OK */ - return true; - } - return false; -} - -/*************************************************************************************************/ - -uint32_t bstMasterWriteTimeout = 0; void bstMasterWriteLoop(void) { - while(bstWriteDataLen) { - if(bufferPointer == 0) { - bool scl_set = false; - if(BSTx == I2C1) - scl_set = BST1_SCL_GPIO->IDR&BST1_SCL_PIN; - else - scl_set = BST2_SCL_GPIO->IDR&BST2_SCL_PIN; - - if(I2C_GetFlagStatus(BSTx, I2C_ISR_BUSY)==RESET && scl_set) { - /* Configure slave address, nbytes, reload, end mode and start or stop generation */ - I2C_TransferHandling(BSTx, dataBuffer[bufferPointer], 1, I2C_Reload_Mode, I2C_Generate_Start_Write); - bstMasterWriteTimeout = micros(); - bufferPointer++; - } - } else if(bufferPointer == 1) { - if(I2C_GetFlagStatus(BSTx, I2C_ISR_TXIS)==SET) { - /* Send Register len */ - I2C_SendData(BSTx, (uint8_t) dataBuffer[bufferPointer]); - bstMasterWriteTimeout = micros(); - } else if(I2C_GetFlagStatus(BSTx, I2C_ISR_TCR)==SET) { - /* Configure slave address, nbytes, reload, end mode and start or stop generation */ - I2C_TransferHandling(BSTx, dataBuffer[bufferPointer-1], dataBuffer[bufferPointer], I2C_AutoEnd_Mode, I2C_No_StartStop); - bstMasterWriteTimeout = micros(); - bufferPointer++; - } - } else if(bufferPointer == bstWriteDataLen) { - if(I2C_GetFlagStatus(BSTx, I2C_ISR_STOPF)==SET) { - I2C_ClearFlag(BSTx, I2C_ICR_STOPCF); - bstWriteDataLen = 0; - bufferPointer = 0; - } - } else { - if(I2C_GetFlagStatus(BSTx, I2C_ISR_TXIS)==SET) { - I2C_SendData(BSTx, (uint8_t) dataBuffer[bufferPointer]); - bstMasterWriteTimeout = micros(); - bufferPointer++; - } - } - uint32_t currentTime = micros(); - if(currentTime>bstMasterWriteTimeout+5000) { - I2C_SoftwareResetCmd(BSTx); - bstWriteDataLen = 0; - bufferPointer = 0; + static uint32_t bstMasterWriteTimeout = 0; + uint32_t currentTime = micros(); + if(bstWriteDataLen && dataBufferPointer==0) { + bool scl_set = false; + if(BSTx == I2C1) + scl_set = BST1_SCL_GPIO->IDR&BST1_SCL_PIN; + else + scl_set = BST2_SCL_GPIO->IDR&BST2_SCL_PIN; + if(I2C_GetFlagStatus(BSTx, I2C_FLAG_BUSY)==RESET && scl_set) { + I2C_TransferHandling(BSTx, dataBuffer[dataBufferPointer], dataBuffer[dataBufferPointer+1]+1, I2C_AutoEnd_Mode, I2C_Generate_Start_Write); + I2C_ITConfig(BSTx, I2C_IT_TXI, ENABLE); + dataBufferPointer = 1; + bstMasterWriteTimeout = micros(); } + } else if(currentTime>bstMasterWriteTimeout+BST_SHORT_TIMEOUT) { + bstTimeoutUserCallback(); } } -void bstMasterReadLoop(void) -{ -} /*************************************************************************************************/ void crc8Cal(uint8_t data_in) { diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h index 64ec839908..c7ceae8e73 100644 --- a/src/main/drivers/nvic.h +++ b/src/main/drivers/nvic.h @@ -8,6 +8,7 @@ #define NVIC_PRIO_TIMER NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_BARO_EXT NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_WS2811_DMA NVIC_BUILD_PRIORITY(1, 2) // TODO - is there some reason to use high priority? (or to use DMA IRQ at all?) +#define NVIC_PRIO_BST_READ_DATA NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_TRANSPONDER_DMA NVIC_BUILD_PRIORITY(3, 0) #define NVIC_PRIO_SERIALUART1_TXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART1_RXDMA NVIC_BUILD_PRIORITY(1, 1) diff --git a/src/main/io/i2c_bst.c b/src/main/io/i2c_bst.c index f45628ca9d..453c9c8c51 100644 --- a/src/main/io/i2c_bst.c +++ b/src/main/io/i2c_bst.c @@ -290,7 +290,6 @@ static const char pidnames[] = "MAG;" "VEL;"; -#define DATA_BUFFER_SIZE 64 #define BOARD_IDENTIFIER_LENGTH 4 typedef struct box_e { @@ -332,8 +331,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; -uint8_t readData[DATA_BUFFER_SIZE]; -uint8_t writeData[DATA_BUFFER_SIZE]; +extern uint8_t readData[DATA_BUFFER_SIZE]; +extern uint8_t writeData[DATA_BUFFER_SIZE]; /*************************************************************************************************/ uint8_t writeBufferPointer = 1; @@ -354,7 +353,12 @@ static void bstWrite32(uint32_t data) bstWrite16((uint16_t)(data >> 16)); } -uint8_t readBufferPointer = 3; +uint8_t readBufferPointer = 4; +static uint8_t bstCurrentAddress(void) +{ + return readData[0]; +} + static uint8_t bstRead8(void) { return readData[readBufferPointer++] & 0xff; @@ -376,12 +380,12 @@ static uint32_t bstRead32(void) static uint8_t bstReadDataSize(void) { - return readData[0]-4; + return readData[1]-5; } static uint8_t bstReadCRC(void) { - return readData[readData[0]]; + return readData[readData[1]+1]; } static void s_struct(uint8_t *cb, uint8_t siz) @@ -495,6 +499,7 @@ static void bstWriteDataflashReadReply(uint32_t address, uint8_t size) /*************************************************************************************************/ #define BST_USB_COMMANDS 0x0A +#define BST_GENERAL_HEARTBEAT 0x0B #define BST_USB_DEVICE_INFO_REQUEST 0x04 //Handshake #define BST_USB_DEVICE_INFO_FRAME 0x05 //Handshake #define BST_READ_COMMANDS 0x26 @@ -999,7 +1004,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) // we do not know how to handle the (valid) message, indicate error BST return false; } - bstSlaveWrite(writeData); + //bstSlaveWrite(writeData); return true; } @@ -1236,7 +1241,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) if (ARMING_FLAG(ARMED)) { ret = BST_FAILED; bstWrite8(ret); - bstSlaveWrite(writeData); + //bstSlaveWrite(writeData); return ret; } writeEEPROM(); @@ -1465,7 +1470,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) ret = BST_FAILED; } bstWrite8(ret); - bstSlaveWrite(writeData); + //bstSlaveWrite(writeData); if(ret == BST_FAILED) return false; return true; @@ -1482,18 +1487,19 @@ static bool bstSlaveUSBCommandFeedback(/*uint8_t bstFeedback*/) bstWrite8(FC_VERSION_MINOR); //Firmware ID bstWrite8(0x00); bstWrite8(0x00); - bstSlaveWrite(writeData); + //bstSlaveWrite(writeData); return true; } /*************************************************************************************************/ -bool slaveModeOn = false; -static void bstSlaveProcessInCommand(void) +#define BST_RESET_TIME 1.2*1000*1000 //micro-seconds +uint32_t resetBstTimer = 0; +bool needResetCheck = true; + +void bstProcessInCommand(void) { - if(bstSlaveRead(readData)) { - slaveModeOn = true; - readBufferPointer = 1; - //Check if the CRC match + readBufferPointer = 2; + if(bstCurrentAddress() == CLEANFLIGHT_FC) { if(bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) { uint8_t i; writeBufferPointer = 1; @@ -1519,8 +1525,23 @@ static void bstSlaveProcessInCommand(void) break; } } - } else { - slaveModeOn = false; + } else if(bstCurrentAddress() == 0x00) { + if(bstReadCRC() == CRC8 && bstRead8()==BST_GENERAL_HEARTBEAT) { + resetBstTimer = micros(); + needResetCheck = true; + } + } +} + +void resetBstChecker(void) +{ + if(needResetCheck) { + uint32_t currentTimer = micros(); + if(currentTimer >= (resetBstTimer + BST_RESET_TIME)) + { + bstTimeoutUserCallback(); + needResetCheck = false; + } } } @@ -1555,26 +1576,13 @@ void taskBstMasterProcess(void) if(sensors(SENSOR_GPS) && !bstWriteBusy()) writeGpsPositionPrameToBST(); } -} - -void taskBstCheckCommand(void) -{ - //Check if the BST input command available to out address - bstSlaveProcessInCommand(); - + bstMasterWriteLoop(); if (isRebootScheduled) { stopMotors(); handleOneshotFeatureChangeOnRestart(); systemReset(); } -} - -void bstMasterWriteLoop(void); -void taskBstReadWrite(void) -{ - taskBstCheckCommand(); - if(!slaveModeOn) - bstMasterWriteLoop(); + resetBstChecker(); } /*************************************************************************************************/ diff --git a/src/main/io/i2c_bst.h b/src/main/io/i2c_bst.h index 366406d69c..d2678764ec 100644 --- a/src/main/io/i2c_bst.h +++ b/src/main/io/i2c_bst.h @@ -19,13 +19,10 @@ #include "drivers/bus_bst.h" -void taskBstReadWrite(void); +void bstProcessInCommand(void); +void bstSlaveProcessInCommand(void); void taskBstMasterProcess(void); -void taskBstCheckCommand(void); -//void writeGpsPositionPrameToBST(void); -//void writeGPSTimeFrameToBST(void); -//void writeDataToBST(void); bool writeGpsPositionPrameToBST(void); bool writeRollPitchYawToBST(void); bool writeRCChannelToBST(void); diff --git a/src/main/main.c b/src/main/main.c index f75dc570ef..1450ae07cd 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -725,7 +725,6 @@ int main(void) { setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); #endif #ifdef USE_BST - setTaskEnabled(TASK_BST_READ_WRITE, true); setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif diff --git a/src/main/scheduler.h b/src/main/scheduler.h index f91bca78a3..34141fd820 100755 --- a/src/main/scheduler.h +++ b/src/main/scheduler.h @@ -81,7 +81,6 @@ typedef enum { #endif #ifdef USE_BST - TASK_BST_READ_WRITE, TASK_BST_MASTER_PROCESS, #endif diff --git a/src/main/scheduler_tasks.c b/src/main/scheduler_tasks.c index a937537c45..223eadc03d 100644 --- a/src/main/scheduler_tasks.c +++ b/src/main/scheduler_tasks.c @@ -188,13 +188,6 @@ cfTask_t cfTasks[TASK_COUNT] = { #endif #ifdef USE_BST - [TASK_BST_READ_WRITE] = { - .taskName = "BST_MASTER_WRITE", - .taskFunc = taskBstReadWrite, - .desiredPeriod = 1000000 / 100, // 100 Hz - .staticPriority = TASK_PRIORITY_IDLE, - }, - [TASK_BST_MASTER_PROCESS] = { .taskName = "BST_MASTER_PROCESS", .taskFunc = taskBstMasterProcess,