From 4d481e9c1ffebbda811a7fca18f82e3880e38fc0 Mon Sep 17 00:00:00 2001 From: "Larry (TBS)" Date: Wed, 23 Mar 2016 19:22:06 +0800 Subject: [PATCH 1/2] - BST change to i2c interrupt - Add to receive heartbeat from CorePro otherwise, will soft reset the i2c. --- src/main/drivers/bus_bst.h | 4 +- src/main/drivers/bus_bst_stm32f30x.c | 434 +++++++++++++-------------- src/main/drivers/nvic.h | 1 + src/main/io/i2c_bst.c | 74 +++-- src/main/io/i2c_bst.h | 7 +- src/main/main.c | 1 - src/main/scheduler.h | 1 - src/main/scheduler_tasks.c | 7 - 8 files changed, 252 insertions(+), 277 deletions(-) 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, From 0cae020f768ef12deb59803c3638d84c0eb34854 Mon Sep 17 00:00:00 2001 From: "Larry (TBS)" Date: Tue, 29 Mar 2016 15:39:19 +0800 Subject: [PATCH 2/2] - MultiFlasher support for SimonK escs --- Makefile | 1 + src/main/drivers/serial_escserial.c | 888 ++++++++++++++++++++++++++ src/main/drivers/serial_escserial.h | 36 ++ src/main/io/serial_cli.c | 56 ++ src/main/io/serial_msp.c | 47 ++ src/main/io/serial_msp.h | 1 + src/main/target/COLIBRI_RACE/target.h | 3 + 7 files changed, 1032 insertions(+) create mode 100755 src/main/drivers/serial_escserial.c create mode 100755 src/main/drivers/serial_escserial.h diff --git a/Makefile b/Makefile index 7ee64c6fea..384b030358 100644 --- a/Makefile +++ b/Makefile @@ -604,6 +604,7 @@ COLIBRI_RACE_SRC = \ drivers/display_ug2864hsweg01.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_escserial.c \ drivers/serial_usb_vcp.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) \ diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c new file mode 100755 index 0000000000..fe260bae5a --- /dev/null +++ b/src/main/drivers/serial_escserial.c @@ -0,0 +1,888 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_ESCSERIAL) + +#include "build_config.h" + +#include "common/utils.h" +#include "common/atomic.h" +#include "common/printf.h" + +#include "nvic.h" +#include "system.h" +#include "gpio.h" +#include "timer.h" + +#include "serial.h" +#include "serial_escserial.h" +#include "drivers/light_led.h" +#include "io/serial.h" +#include "flight/mixer.h" + +#define RX_TOTAL_BITS 10 +#define TX_TOTAL_BITS 10 + +#define MAX_ESCSERIAL_PORTS 1 +static serialPort_t *escPort = NULL; +static serialPort_t *passPort = NULL; + +typedef struct escSerial_s { + serialPort_t port; + + const timerHardware_t *rxTimerHardware; + volatile uint8_t rxBuffer[ESCSERIAL_BUFFER_SIZE]; + const timerHardware_t *txTimerHardware; + volatile uint8_t txBuffer[ESCSERIAL_BUFFER_SIZE]; + + uint8_t isSearchingForStartBit; + uint8_t rxBitIndex; + uint8_t rxLastLeadingEdgeAtBitIndex; + uint8_t rxEdge; + + uint8_t isTransmittingData; + uint8_t isReceivingData; + int8_t bitsLeftToTransmit; + + uint16_t internalTxBuffer; // includes start and stop bits + uint16_t internalRxBuffer; // includes start and stop bits + + uint16_t receiveTimeout; + uint16_t transmissionErrors; + uint16_t receiveErrors; + + uint8_t escSerialPortIndex; + + timerCCHandlerRec_t timerCb; + timerCCHandlerRec_t edgeCb; +} escSerial_t; + +extern timerHardware_t* serialTimerHardware; +extern escSerial_t escSerialPorts[]; + +extern const struct serialPortVTable escSerialVTable[]; + + +escSerial_t escSerialPorts[MAX_ESCSERIAL_PORTS]; + +void onEscSerialTimer(timerCCHandlerRec_t *cbRec, captureCompare_t capture); +void onEscSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture); +void onEscSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); +void onEscSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); +static void escSerialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); + +void setEscTxSignal(escSerial_t *escSerial, uint8_t state) +{ + if (state) { + digitalHi(escSerial->rxTimerHardware->gpio, escSerial->rxTimerHardware->pin); + } else { + digitalLo(escSerial->rxTimerHardware->gpio, escSerial->rxTimerHardware->pin); + } +} + +static void escSerialGPIOConfig(GPIO_TypeDef *gpio, uint16_t pin, GPIO_Mode mode) +{ + gpio_config_t cfg; + + cfg.pin = pin; + cfg.mode = mode; + cfg.speed = Speed_2MHz; + gpioInit(gpio, &cfg); +} + +void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr) +{ +#ifdef STM32F10X + escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU); +#else + escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_AF_PP_PU); +#endif + //escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode); + timerChClearCCFlag(timerHardwarePtr); + timerChITConfig(timerHardwarePtr,ENABLE); +} + + +static bool isTimerPeriodTooLarge(uint32_t timerPeriod) +{ + return timerPeriod > 0xFFFF; +} + +static void escSerialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, uint32_t baud) +{ + uint32_t clock = SystemCoreClock/2; + uint32_t timerPeriod; + TIM_DeInit(timerHardwarePtr->tim); + do { + timerPeriod = clock / baud; + if (isTimerPeriodTooLarge(timerPeriod)) { + if (clock > 1) { + clock = clock / 2; // this is wrong - mhz stays the same ... This will double baudrate until ok (but minimum baudrate is < 1200) + } else { + // TODO unable to continue, unable to determine clock and timerPeriods for the given baud + } + + } + } while (isTimerPeriodTooLarge(timerPeriod)); + + uint8_t mhz = clock / 1000000; + timerConfigure(timerHardwarePtr, timerPeriod, mhz); + timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onEscSerialTimerBL); + timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); +} + +static void escSerialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_t options) +{ + // start bit is usually a FALLING signal + uint8_t mhz = SystemCoreClock / 2000000; + TIM_DeInit(timerHardwarePtr->tim); + timerConfigure(timerHardwarePtr, 0xFFFF, mhz); + escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); + timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onEscSerialRxPinChangeBL); + timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); +} + +static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) +{ + uint32_t timerPeriod=34; + TIM_DeInit(timerHardwarePtr->tim); + timerConfigure(timerHardwarePtr, timerPeriod, 1); + timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onEscSerialTimer); + timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); +} + +static void escSerialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) +{ + TIM_ICInitTypeDef TIM_ICInitStructure; + + TIM_ICStructInit(&TIM_ICInitStructure); + TIM_ICInitStructure.TIM_Channel = channel; + TIM_ICInitStructure.TIM_ICPolarity = polarity; + TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; + TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; + TIM_ICInitStructure.TIM_ICFilter = 0x0; + + TIM_ICInit(tim, &TIM_ICInitStructure); +} + +static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) +{ + // start bit is usually a FALLING signal + TIM_DeInit(timerHardwarePtr->tim); + timerConfigure(timerHardwarePtr, 0xFFFF, 1); + escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); + timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onEscSerialRxPinChange); + timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); +} + +static void escSerialOutputPortConfig(const timerHardware_t *timerHardwarePtr) +{ + escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_Out_PP); + timerChITConfig(timerHardwarePtr,DISABLE); +} + +static void resetBuffers(escSerial_t *escSerial) +{ + escSerial->port.rxBufferSize = ESCSERIAL_BUFFER_SIZE; + escSerial->port.rxBuffer = escSerial->rxBuffer; + escSerial->port.rxBufferTail = 0; + escSerial->port.rxBufferHead = 0; + + escSerial->port.txBuffer = escSerial->txBuffer; + escSerial->port.txBufferSize = ESCSERIAL_BUFFER_SIZE; + escSerial->port.txBufferTail = 0; + escSerial->port.txBufferHead = 0; +} + +serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode) +{ + escSerial_t *escSerial = &(escSerialPorts[portIndex]); + + escSerial->rxTimerHardware = &(timerHardware[output]); + escSerial->txTimerHardware = &(timerHardware[ESCSERIAL_TIMER_TX_HARDWARE]); + + escSerial->port.vTable = escSerialVTable; + escSerial->port.baudRate = baud; + escSerial->port.mode = MODE_RXTX; + escSerial->port.options = options; + escSerial->port.callback = callback; + + resetBuffers(escSerial); + + escSerial->isTransmittingData = false; + + escSerial->isSearchingForStartBit = true; + escSerial->rxBitIndex = 0; + + escSerial->transmissionErrors = 0; + escSerial->receiveErrors = 0; + escSerial->receiveTimeout = 0; + + escSerial->escSerialPortIndex = portIndex; + + escSerialInputPortConfig(escSerial->rxTimerHardware); + + setEscTxSignal(escSerial, ENABLE); + delay(50); + + if(mode==0){ + escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex); + escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex); + } + if(mode==1){ + escSerialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); + escSerialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options); + } + + return &escSerial->port; +} + + +void escSerialInputPortDeConfig(const timerHardware_t *timerHardwarePtr) +{ + timerChClearCCFlag(timerHardwarePtr); + timerChITConfig(timerHardwarePtr,DISABLE); + escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU); +} + + +void closeEscSerial(escSerialPortIndex_e portIndex, uint16_t output) +{ + escSerial_t *escSerial = &(escSerialPorts[portIndex]); + + escSerial->rxTimerHardware = &(timerHardware[output]); + escSerial->txTimerHardware = &(timerHardware[ESCSERIAL_TIMER_TX_HARDWARE]); + escSerialInputPortDeConfig(escSerial->rxTimerHardware); + timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL); + timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL); + TIM_DeInit(escSerial->txTimerHardware->tim); + TIM_DeInit(escSerial->rxTimerHardware->tim); +} + +/*********************************************/ + +void processEscTxState(escSerial_t *escSerial) +{ + uint8_t mask; + static uint8_t bitq=0, transmitStart=0; + if (escSerial->isReceivingData) { + return; + } + + if(transmitStart==0) + { + setEscTxSignal(escSerial, 1); + } + if (!escSerial->isTransmittingData) { + char byteToSend; +reload: + if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { + // canreceive + transmitStart=0; + return; + } + + if(transmitStart<3) + { + if(transmitStart==0) + byteToSend = 0xff; + if(transmitStart==1) + byteToSend = 0xff; + if(transmitStart==2) + byteToSend = 0x7f; + transmitStart++; + } + else{ + // data to send + byteToSend = escSerial->port.txBuffer[escSerial->port.txBufferTail++]; + if (escSerial->port.txBufferTail >= escSerial->port.txBufferSize) { + escSerial->port.txBufferTail = 0; + } + } + + + // build internal buffer, data bits (MSB to LSB) + escSerial->internalTxBuffer = byteToSend; + escSerial->bitsLeftToTransmit = 8; + escSerial->isTransmittingData = true; + + //set output + escSerialOutputPortConfig(escSerial->rxTimerHardware); + return; + } + + if (escSerial->bitsLeftToTransmit) { + mask = escSerial->internalTxBuffer & 1; + if(mask) + { + if(bitq==0 || bitq==1) + { + setEscTxSignal(escSerial, 1); + } + if(bitq==2 || bitq==3) + { + setEscTxSignal(escSerial, 0); + } + } + else + { + if(bitq==0 || bitq==2) + { + setEscTxSignal(escSerial, 1); + } + if(bitq==1 ||bitq==3) + { + setEscTxSignal(escSerial, 0); + } + } + bitq++; + if(bitq>3) + { + escSerial->internalTxBuffer >>= 1; + escSerial->bitsLeftToTransmit--; + bitq=0; + if(escSerial->bitsLeftToTransmit==0) + { + goto reload; + } + } + return; + } + + if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { + escSerial->isTransmittingData = false; + escSerialInputPortConfig(escSerial->rxTimerHardware); + } +} + +/*-----------------------BL*/ +/*********************************************/ + +void processEscTxStateBL(escSerial_t *escSerial) +{ + uint8_t mask; + if (escSerial->isReceivingData) { + return; + } + + if (!escSerial->isTransmittingData) { + char byteToSend; + if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { + // canreceive + return; + } + + // data to send + byteToSend = escSerial->port.txBuffer[escSerial->port.txBufferTail++]; + if (escSerial->port.txBufferTail >= escSerial->port.txBufferSize) { + escSerial->port.txBufferTail = 0; + } + + // build internal buffer, MSB = Stop Bit (1) + data bits (MSB to LSB) + start bit(0) LSB + escSerial->internalTxBuffer = (1 << (TX_TOTAL_BITS - 1)) | (byteToSend << 1); + escSerial->bitsLeftToTransmit = TX_TOTAL_BITS; + escSerial->isTransmittingData = true; + + + //set output + escSerialOutputPortConfig(escSerial->rxTimerHardware); + return; + } + + if (escSerial->bitsLeftToTransmit) { + mask = escSerial->internalTxBuffer & 1; + escSerial->internalTxBuffer >>= 1; + + setEscTxSignal(escSerial, mask); + escSerial->bitsLeftToTransmit--; + return; + } + + escSerial->isTransmittingData = false; + if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { + escSerialInputPortConfig(escSerial->rxTimerHardware); + } +} + + + +enum { + TRAILING, + LEADING +}; + +void applyChangedBitsEscBL(escSerial_t *escSerial) +{ + if (escSerial->rxEdge == TRAILING) { + uint8_t bitToSet; + for (bitToSet = escSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < escSerial->rxBitIndex; bitToSet++) { + escSerial->internalRxBuffer |= 1 << bitToSet; + } + } +} + +void prepareForNextEscRxByteBL(escSerial_t *escSerial) +{ + // prepare for next byte + escSerial->rxBitIndex = 0; + escSerial->isSearchingForStartBit = true; + if (escSerial->rxEdge == LEADING) { + escSerial->rxEdge = TRAILING; + escSerialICConfig( + escSerial->rxTimerHardware->tim, + escSerial->rxTimerHardware->channel, + (escSerial->port.options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling + ); + } +} + +#define STOP_BIT_MASK (1 << 0) +#define START_BIT_MASK (1 << (RX_TOTAL_BITS - 1)) + +void extractAndStoreEscRxByteBL(escSerial_t *escSerial) +{ + if ((escSerial->port.mode & MODE_RX) == 0) { + return; + } + + uint8_t haveStartBit = (escSerial->internalRxBuffer & START_BIT_MASK) == 0; + uint8_t haveStopBit = (escSerial->internalRxBuffer & STOP_BIT_MASK) == 1; + + if (!haveStartBit || !haveStopBit) { + escSerial->receiveErrors++; + return; + } + + uint8_t rxByte = (escSerial->internalRxBuffer >> 1) & 0xFF; + + if (escSerial->port.callback) { + escSerial->port.callback(rxByte); + } else { + escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte; + escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize; + } +} + +void processEscRxStateBL(escSerial_t *escSerial) +{ + if (escSerial->isSearchingForStartBit) { + return; + } + + escSerial->rxBitIndex++; + + if (escSerial->rxBitIndex == RX_TOTAL_BITS - 1) { + applyChangedBitsEscBL(escSerial); + return; + } + + if (escSerial->rxBitIndex == RX_TOTAL_BITS) { + + if (escSerial->rxEdge == TRAILING) { + escSerial->internalRxBuffer |= STOP_BIT_MASK; + } + + extractAndStoreEscRxByteBL(escSerial); + prepareForNextEscRxByteBL(escSerial); + } +} + +void onEscSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture) +{ + UNUSED(capture); + escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb); + + processEscTxStateBL(escSerial); + processEscRxStateBL(escSerial); +} + +void onEscSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture) +{ + UNUSED(capture); + + escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb); + bool inverted = escSerial->port.options & SERIAL_INVERTED; + + if ((escSerial->port.mode & MODE_RX) == 0) { + return; + } + + if (escSerial->isSearchingForStartBit) { + // synchronise bit counter + // FIXME this reduces functionality somewhat as receiving breaks concurrent transmission on all ports because + // the next callback to the onSerialTimer will happen too early causing transmission errors. + TIM_SetCounter(escSerial->txTimerHardware->tim, escSerial->txTimerHardware->tim->ARR / 2); + if (escSerial->isTransmittingData) { + escSerial->transmissionErrors++; + } + + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); + escSerial->rxEdge = LEADING; + + escSerial->rxBitIndex = 0; + escSerial->rxLastLeadingEdgeAtBitIndex = 0; + escSerial->internalRxBuffer = 0; + escSerial->isSearchingForStartBit = false; + return; + } + + if (escSerial->rxEdge == LEADING) { + escSerial->rxLastLeadingEdgeAtBitIndex = escSerial->rxBitIndex; + } + + applyChangedBitsEscBL(escSerial); + + if (escSerial->rxEdge == TRAILING) { + escSerial->rxEdge = LEADING; + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); + } else { + escSerial->rxEdge = TRAILING; + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); + } +} +/*-------------------------BL*/ + +void extractAndStoreEscRxByte(escSerial_t *escSerial) +{ + if ((escSerial->port.mode & MODE_RX) == 0) { + return; + } + + uint8_t rxByte = (escSerial->internalRxBuffer) & 0xFF; + + if (escSerial->port.callback) { + escSerial->port.callback(rxByte); + } else { + escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte; + escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize; + } +} + +void onEscSerialTimer(timerCCHandlerRec_t *cbRec, captureCompare_t capture) +{ + UNUSED(capture); + escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb); + + if(escSerial->isReceivingData) + { + escSerial->receiveTimeout++; + if(escSerial->receiveTimeout>8) + { + escSerial->isReceivingData=0; + escSerial->receiveTimeout=0; + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Falling); + } + } + + + processEscTxState(escSerial); +} + +void onEscSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) +{ + UNUSED(capture); + static uint8_t zerofirst=0; + static uint8_t bits=0; + static uint16_t bytes=0; + + escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb); + + //clear timer + TIM_SetCounter(escSerial->rxTimerHardware->tim,0); + + if(capture > 40 && capture < 90) + { + zerofirst++; + if(zerofirst>1) + { + zerofirst=0; + escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1; + bits++; + } + } + else if(capture>90 && capture < 200) + { + zerofirst=0; + escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1; + escSerial->internalRxBuffer |= 0x80; + bits++; + } + else + { + if(!escSerial->isReceivingData) + { + //start + //lets reset + + escSerial->isReceivingData = 1; + zerofirst=0; + bytes=0; + bits=1; + escSerial->internalRxBuffer = 0x80; + + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Rising); + } + } + escSerial->receiveTimeout = 0; + + if(bits==8) + { + bits=0; + bytes++; + if(bytes>3) + { + extractAndStoreEscRxByte(escSerial); + } + escSerial->internalRxBuffer=0; + } + +} + +uint8_t escSerialTotalBytesWaiting(serialPort_t *instance) +{ + if ((instance->mode & MODE_RX) == 0) { + return 0; + } + + escSerial_t *s = (escSerial_t *)instance; + + return (s->port.rxBufferHead - s->port.rxBufferTail) & (s->port.rxBufferSize - 1); +} + +uint8_t escSerialReadByte(serialPort_t *instance) +{ + uint8_t ch; + + if ((instance->mode & MODE_RX) == 0) { + return 0; + } + + if (escSerialTotalBytesWaiting(instance) == 0) { + return 0; + } + + ch = instance->rxBuffer[instance->rxBufferTail]; + instance->rxBufferTail = (instance->rxBufferTail + 1) % instance->rxBufferSize; + return ch; +} + +void escSerialWriteByte(serialPort_t *s, uint8_t ch) +{ + if ((s->mode & MODE_TX) == 0) { + return; + } + + s->txBuffer[s->txBufferHead] = ch; + s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize; +} + +void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate) +{ + UNUSED(s); + UNUSED(baudRate); +} + +void escSerialSetMode(serialPort_t *instance, portMode_t mode) +{ + instance->mode = mode; +} + +bool isEscSerialTransmitBufferEmpty(serialPort_t *instance) +{ + // start listening + return instance->txBufferHead == instance->txBufferTail; +} + +uint8_t escSerialTxBytesFree(serialPort_t *instance) +{ + if ((instance->mode & MODE_TX) == 0) { + return 0; + } + + escSerial_t *s = (escSerial_t *)instance; + + uint8_t bytesUsed = (s->port.txBufferHead - s->port.txBufferTail) & (s->port.txBufferSize - 1); + + return (s->port.txBufferSize - 1) - bytesUsed; +} + +const struct serialPortVTable escSerialVTable[] = { + { + escSerialWriteByte, + escSerialTotalBytesWaiting, + escSerialTxBytesFree, + escSerialReadByte, + escSerialSetBaudRate, + isEscSerialTransmitBufferEmpty, + escSerialSetMode, + .writeBuf = NULL, + } +}; + +void escSerialInitialize() +{ + StopPwmAllMotors(); + for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + // set outputs to pullup + if(timerHardware[i].outputEnable==1) + { + escSerialGPIOConfig(timerHardware[i].gpio,timerHardware[i].pin, Mode_IPU); //GPIO_Mode_IPU + } + } +} + +typedef enum { + IDLE, + HEADER_START, + HEADER_M, + HEADER_ARROW, + HEADER_SIZE, + HEADER_CMD, + COMMAND_RECEIVED +} mspState_e; + +typedef struct mspPort_s { + uint8_t offset; + uint8_t dataSize; + uint8_t checksum; + uint8_t indRX; + uint8_t inBuf[10]; + mspState_e c_state; + uint8_t cmdMSP; +} mspPort_t; + +static mspPort_t currentPort; + +static bool ProcessExitCommand(uint8_t c) +{ + if (currentPort.c_state == IDLE) { + if (c == '$') { + currentPort.c_state = HEADER_START; + } else { + return false; + } + } else if (currentPort.c_state == HEADER_START) { + currentPort.c_state = (c == 'M') ? HEADER_M : IDLE; + } else if (currentPort.c_state == HEADER_M) { + currentPort.c_state = (c == '<') ? HEADER_ARROW : IDLE; + } else if (currentPort.c_state == HEADER_ARROW) { + if (c > 10) { + currentPort.c_state = IDLE; + + } else { + currentPort.dataSize = c; + currentPort.offset = 0; + currentPort.checksum = 0; + currentPort.indRX = 0; + currentPort.checksum ^= c; + currentPort.c_state = HEADER_SIZE; + } + } else if (currentPort.c_state == HEADER_SIZE) { + currentPort.cmdMSP = c; + currentPort.checksum ^= c; + currentPort.c_state = HEADER_CMD; + } else if (currentPort.c_state == HEADER_CMD && currentPort.offset < currentPort.dataSize) { + currentPort.checksum ^= c; + currentPort.inBuf[currentPort.offset++] = c; + } else if (currentPort.c_state == HEADER_CMD && currentPort.offset >= currentPort.dataSize) { + if (currentPort.checksum == c) { + currentPort.c_state = COMMAND_RECEIVED; + + if((currentPort.cmdMSP == 0xF4) && (currentPort.dataSize==0)) + { + currentPort.c_state = IDLE; + return true; + } + } else { + currentPort.c_state = IDLE; + } + } + return false; +} + + +// mode 0=sk, mode 1=bl output=timerHardware PWM channel. +void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode) +{ + bool exitEsc = false; + LED0_OFF; + LED1_OFF; + StopPwmAllMotors(); + passPort = escPassthroughPort; + + uint8_t first_output = 0; + for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + if(timerHardware[i].outputEnable==1) + { + first_output=i; + break; + } + } + + //doesn't work with messy timertable + uint8_t motor_output=first_output+output-1; + if(motor_output >=USABLE_TIMER_CHANNEL_COUNT) + return; + + escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, 19200, 0, mode); + uint8_t ch; + while(1) { + if (serialRxBytesWaiting(escPort)) { + LED0_ON; + while(serialRxBytesWaiting(escPort)) + { + ch = serialRead(escPort); + serialWrite(escPassthroughPort, ch); + } + LED0_OFF; + } + if (serialRxBytesWaiting(escPassthroughPort)) { + LED1_ON; + while(serialRxBytesWaiting(escPassthroughPort)) + { + ch = serialRead(escPassthroughPort); + exitEsc = ProcessExitCommand(ch); + if(exitEsc) + { + serialWrite(escPassthroughPort, 0x24); + serialWrite(escPassthroughPort, 0x4D); + serialWrite(escPassthroughPort, 0x3E); + serialWrite(escPassthroughPort, 0x00); + serialWrite(escPassthroughPort, 0xF4); + serialWrite(escPassthroughPort, 0xF4); + closeEscSerial(ESCSERIAL1, output); + return; + } + if(mode){ + serialWrite(escPassthroughPort, ch); // blheli loopback + } + serialWrite(escPort, ch); + } + LED1_OFF; + } + delay(5); + } +} + + +#endif diff --git a/src/main/drivers/serial_escserial.h b/src/main/drivers/serial_escserial.h new file mode 100755 index 0000000000..ab4a1abd3d --- /dev/null +++ b/src/main/drivers/serial_escserial.h @@ -0,0 +1,36 @@ +/* + * 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 ESCSERIAL_BUFFER_SIZE 1024 + +typedef enum { + ESCSERIAL1 = 0, + ESCSERIAL2 +} escSerialPortIndex_e; + +serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode); + +// serialPort API +void escSerialWriteByte(serialPort_t *instance, uint8_t ch); +uint8_t escSerialTotalBytesWaiting(serialPort_t *instance); +uint8_t escSerialReadByte(serialPort_t *instance); +void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate); +bool isEscSerialTransmitBufferEmpty(serialPort_t *s); +void escSerialInitialize(); +void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3e4d761c8b..cd73f34331 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -141,6 +141,9 @@ static void cliRxRange(char *cmdline); #ifdef GPS static void cliGpsPassthrough(char *cmdline); #endif +#ifdef USE_ESCSERIAL +static void cliEscPassthrough(char *cmdline); +#endif static void cliHelp(char *cmdline); static void cliMap(char *cmdline); @@ -271,6 +274,9 @@ const clicmd_t cmdTable[] = { "[name]", cliGet), #ifdef GPS CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough), +#endif +#ifdef USE_ESCSERIAL + CLI_COMMAND_DEF("escprog", "passthrough esc to serial", " ", cliEscPassthrough), #endif CLI_COMMAND_DEF("help", NULL, NULL, cliHelp), #ifdef LED_STRIP @@ -2192,6 +2198,56 @@ static void cliGpsPassthrough(char *cmdline) } #endif +#ifdef USE_ESCSERIAL +static void cliEscPassthrough(char *cmdline) +{ + uint8_t mode = 0; + int index = 0; + int i = 0; + char *pch = NULL; + char *saveptr; + + if (isEmpty(cmdline)) { + cliShowParseError(); + return; + } + + pch = strtok_r(cmdline, " ", &saveptr); + while (pch != NULL) { + switch (i) { + case 0: + if(strncasecmp(pch, "sk", strlen(pch)) == 0) + { + mode = 0; + } + else if(strncasecmp(pch, "bl", strlen(pch)) == 0) + { + mode = 1; + } + else + { + cliShowParseError(); + return; + } + break; + case 1: + index = atoi(pch); + if ((index >= 0) && (index < USABLE_TIMER_CHANNEL_COUNT)) { + printf("passthru at pwm output %d enabled\r\n", index); + } + else { + printf("invalid pwm output, valid range: 0 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT); + return; + } + break; + } + i++; + pch = strtok_r(NULL, " ", &saveptr); + } + escEnablePassthrough(cliPort,index,mode); +} +#endif + static void cliHelp(char *cmdline) { uint32_t i = 0; diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 37460af95a..3e9742e8d7 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -95,6 +95,9 @@ #ifdef USE_SERIAL_1WIRE #include "io/serial_1wire.h" #endif +#ifdef USE_ESCSERIAL +#include "drivers/serial_escserial.h" +#endif static serialPort_t *mspSerialPort; extern uint16_t cycleTime; // FIXME dependency on mw.c @@ -1841,6 +1844,50 @@ static bool processInCommand(void) } break; #endif + +#ifdef USE_ESCSERIAL + case MSP_SET_ESCSERIAL: + // get channel number + i = read8(); + // we do not give any data back, assume channel number is transmitted OK + if (i == 0xFF) { + // 0xFF -> preinitialize the Passthrough + // switch all motor lines HI + escSerialInitialize(); + + // and come back right afterwards + // rem: App: Wait at least appx. 500 ms for BLHeli to jump into + // bootloader mode before try to connect any ESC + } + else { + // Check for channel number 1..USABLE_TIMER_CHANNEL_COUNT-1 + if ((i > 0) && (i < USABLE_TIMER_CHANNEL_COUNT)) { + // because we do not come back after calling escEnablePassthrough + // proceed with a success reply first + headSerialReply(0); + tailSerialReply(); + // flush the transmit buffer + bufWriterFlush(writer); + // wait for all data to send + while (!isSerialTransmitBufferEmpty(mspSerialPort)) { + delay(50); + } + // Start to activate here + // motor 1 => index 0 + escEnablePassthrough(mspSerialPort,i,0); //sk blmode + // MPS uart is active again + } else { + // ESC channel higher than max. allowed + // rem: BLHeliSuite will not support more than 8 + headSerialError(0); + } + // proceed as usual with MSP commands + // and wait to switch to next channel + // rem: App needs to call MSP_BOOT to deinitialize Passthrough + } + break; +#endif + default: // we do not know how to handle the (valid) message, indicate error MSP $M! return false; diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index d2db79afb5..053a186916 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -255,6 +255,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration #define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration #define MSP_SET_1WIRE 243 //in message Sets 1Wire paththrough +#define MSP_SET_ESCSERIAL 244 //in message Sets escserial passthrough // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports. #define MAX_MSP_PORT_COUNT 2 diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 0fec63c0c8..d267b553c9 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -110,6 +110,9 @@ #define UART3_TX_PINSOURCE GPIO_PinSource10 #define UART3_RX_PINSOURCE GPIO_PinSource11 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define USE_I2C #define I2C_DEVICE (I2CDEV_2)