From 4d481e9c1ffebbda811a7fca18f82e3880e38fc0 Mon Sep 17 00:00:00 2001 From: "Larry (TBS)" Date: Wed, 23 Mar 2016 19:22:06 +0800 Subject: [PATCH 01/78] - 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 23ba48d2f..3ade4baee 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 d63323445..201985e9e 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 64ec83990..c7ceae8e7 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 f45628ca9..453c9c8c5 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 366406d69..d2678764e 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 f75dc570e..1450ae07c 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 f91bca78a..34141fd82 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 a937537c4..223eadc03 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 6eb9922c1c10ce2e41c9e7bcbe7c55a20b431eba Mon Sep 17 00:00:00 2001 From: E Thomas Date: Sat, 26 Mar 2016 22:34:06 -0400 Subject: [PATCH 02/78] Added 'vbat_hysteresis' parameter --- src/main/config/config.c | 1 + src/main/io/serial_cli.c | 1 + src/main/sensors/battery.c | 10 ++++------ src/main/sensors/battery.h | 1 + 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 332c49a72..f438dd2da 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -272,6 +272,7 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig) batteryConfig->vbatmaxcellvoltage = 43; batteryConfig->vbatmincellvoltage = 33; batteryConfig->vbatwarningcellvoltage = 35; + batteryConfig->vbathysteresis = 1; batteryConfig->vbatPidCompensation = 0; batteryConfig->currentMeterOffset = 0; batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3e4d761c8..2033264ab 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -634,6 +634,7 @@ const clivalue_t valueTable[] = { { "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmaxcellvoltage, .config.minmax = { 10, 50 } }, { "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, .config.minmax = { 10, 50 } }, { "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, .config.minmax = { 10, 50 } }, + { "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbathysteresis, .config.minmax = { 0, 250 } }, { "vbat_pid_compensation", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -10000, 10000 } }, { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { 0, 3300 } }, diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index dcdc2167b..5bcc265f1 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -83,8 +83,6 @@ static void updateBatteryVoltage(void) } #define VBATTERY_STABLE_DELAY 40 -/* Batt Hysteresis of +/-100mV */ -#define VBATT_HYSTERESIS 1 void updateBattery(void) { @@ -123,23 +121,23 @@ void updateBattery(void) switch(batteryState) { case BATTERY_OK: - if (vbat <= (batteryWarningVoltage - VBATT_HYSTERESIS)) { + if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) { batteryState = BATTERY_WARNING; beeper(BEEPER_BAT_LOW); } break; case BATTERY_WARNING: - if (vbat <= (batteryCriticalVoltage - VBATT_HYSTERESIS)) { + if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) { batteryState = BATTERY_CRITICAL; beeper(BEEPER_BAT_CRIT_LOW); - } else if (vbat > (batteryWarningVoltage + VBATT_HYSTERESIS)){ + } else if (vbat > (batteryWarningVoltage + batteryConfig->vbathysteresis)){ batteryState = BATTERY_OK; } else { beeper(BEEPER_BAT_LOW); } break; case BATTERY_CRITICAL: - if (vbat > (batteryCriticalVoltage + VBATT_HYSTERESIS)){ + if (vbat > (batteryCriticalVoltage + batteryConfig->vbathysteresis)){ batteryState = BATTERY_WARNING; beeper(BEEPER_BAT_LOW); } else { diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index f4327c065..26e54eb4e 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -40,6 +40,7 @@ typedef struct batteryConfig_s { uint8_t vbatmaxcellvoltage; // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V) uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V) uint8_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V) + uint8_t vbathysteresis; // hysteresis for alarm, default 1 = 0.1V uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A From 0cae020f768ef12deb59803c3638d84c0eb34854 Mon Sep 17 00:00:00 2001 From: "Larry (TBS)" Date: Tue, 29 Mar 2016 15:39:19 +0800 Subject: [PATCH 03/78] - 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 7ee64c6fe..384b03035 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 000000000..fe260bae5 --- /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 000000000..ab4a1abd3 --- /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 3e4d761c8..cd73f3433 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 37460af95..3e9742e8d 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 d2db79afb..053a18691 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 0fec63c0c..d267b553c 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) From 4cc2718d59a839dfddc586c37540df1cfbb4e6da Mon Sep 17 00:00:00 2001 From: mikeller Date: Wed, 30 Mar 2016 01:47:22 +1300 Subject: [PATCH 04/78] Added printing of line of '#' after profile switch during dump to prevent buffer overrun on restore. --- src/main/io/serial_cli.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3e4d761c8..036459842 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1986,6 +1986,7 @@ void cliDumpProfile(uint8_t profileIndex) { changeProfile(profileIndex); cliPrint("\r\n# profile\r\n"); cliProfile(""); + cliPrintf("################################################################################"); printSectionBreak(); dumpValues(PROFILE_VALUE); uint8_t currentRateIndex = currentProfile->activeRateProfile; From 7b468c09f0d14978de4be201125ac4401bdb5776 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 21 Mar 2016 16:53:27 +0100 Subject: [PATCH 05/78] Luxfloat rework to int pids // Many pid cleanups // filter rework Eeprom version // Dterm improvement Further PID Improvements Version Change Coupling configured // reworked filtering // more test features remove iterm scaler luxfloat Further rework filters etc Restore original luxfloat but scaled Restore original luxfloat but scaled --- src/main/blackbox/blackbox.c | 6 +- src/main/common/filter.c | 23 +++++++ src/main/common/filter.h | 4 ++ src/main/config/config.c | 42 +++++------ src/main/flight/pid.c | 130 +++++++++++++---------------------- src/main/flight/pid.h | 6 +- src/main/io/i2c_bst.c | 56 +++------------ src/main/io/rc_controls.c | 110 ++++++++--------------------- src/main/io/serial_cli.c | 15 +--- src/main/io/serial_msp.c | 54 +++------------ src/main/mw.c | 4 +- src/main/version.h | 4 +- 12 files changed, 145 insertions(+), 309 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 65d575903..06c6e34ae 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -401,11 +401,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2: - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { - return currentProfile->pidProfile.D_f[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0; - } else { - return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0; - } + return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0; case FLIGHT_LOG_FIELD_CONDITION_MAG: #ifdef MAG diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 14a9ff30d..8c266a071 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -96,3 +96,26 @@ float applyBiQuadFilter(float sample, biquad_t *state) return result; } + +int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]) { + int count; + int32_t averageSum; + + for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1]; + averageState[0] = input; + for (count = 0; count < averageCount; count++) averageSum += averageState[count]; + + return averageSum / averageCount; +} + +float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]) { + int count; + float averageSum; + + for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1]; + averageState[0] = input; + for (count = 0; count < averageCount; count++) averageSum += averageState[count]; + + return averageSum / averageCount; +} + diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 89b9eae7e..500995602 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -15,6 +15,8 @@ * along with Cleanflight. If not, see . */ +#define DELTA_MAX_SAMPLES 12 + typedef struct filterStatePt1_s { float state; float RC; @@ -29,4 +31,6 @@ typedef struct biquad_s { float applyBiQuadFilter(float sample, biquad_t *state); float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt); +int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]); +float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]); void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate); diff --git a/src/main/config/config.c b/src/main/config/config.c index f438dd2da..846755ff5 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 129; +static const uint8_t EEPROM_CONF_VERSION = 130; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -145,16 +145,16 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) static void resetPidProfile(pidProfile_t *pidProfile) { - pidProfile->pidController = 1; + pidProfile->pidController = 2; - pidProfile->P8[ROLL] = 42; - pidProfile->I8[ROLL] = 40; + pidProfile->P8[ROLL] = 40; + pidProfile->I8[ROLL] = 30; pidProfile->D8[ROLL] = 18; - pidProfile->P8[PITCH] = 54; - pidProfile->I8[PITCH] = 40; - pidProfile->D8[PITCH] = 22; - pidProfile->P8[YAW] = 90; - pidProfile->I8[YAW] = 50; + pidProfile->P8[PITCH] = 40; + pidProfile->I8[PITCH] = 30; + pidProfile->D8[PITCH] = 18; + pidProfile->P8[YAW] = 60; + pidProfile->I8[YAW] = 40; pidProfile->D8[YAW] = 0; pidProfile->P8[PIDALT] = 50; pidProfile->I8[PIDALT] = 0; @@ -168,8 +168,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[PIDNAVR] = 25; // NAV_P * 10; pidProfile->I8[PIDNAVR] = 33; // NAV_I * 100; pidProfile->D8[PIDNAVR] = 83; // NAV_D * 1000; - pidProfile->P8[PIDLEVEL] = 30; - pidProfile->I8[PIDLEVEL] = 30; + pidProfile->P8[PIDLEVEL] = 50; + pidProfile->I8[PIDLEVEL] = 50; pidProfile->D8[PIDLEVEL] = 100; pidProfile->P8[PIDMAG] = 40; pidProfile->P8[PIDVEL] = 120; @@ -177,21 +177,11 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[PIDVEL] = 1; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; + pidProfile->yaw_lpf_hz = 0.0f; pidProfile->dterm_average_count = 4; - pidProfile->dterm_lpf_hz = 0; // filtering ON by default + pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; - pidProfile->P_f[ROLL] = 1.1f; - pidProfile->I_f[ROLL] = 0.4f; - pidProfile->D_f[ROLL] = 0.01f; - pidProfile->P_f[PITCH] = 1.4f; - pidProfile->I_f[PITCH] = 0.4f; - pidProfile->D_f[PITCH] = 0.01f; - pidProfile->P_f[YAW] = 2.5f; - pidProfile->I_f[YAW] = 0.4f; - pidProfile->D_f[YAW] = 0.00f; - pidProfile->A_level = 4.0f; - pidProfile->H_level = 4.0f; pidProfile->H_sensitivity = 75; #ifdef GTUNE @@ -313,7 +303,7 @@ void resetSerialConfig(serialConfig_t *serialConfig) static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcRate8 = 100; - controlRateConfig->rcExpo8 = 70; + controlRateConfig->rcExpo8 = 20; controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; controlRateConfig->dynThrPID = 0; @@ -321,7 +311,7 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->tpa_breakpoint = 1500; for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { - controlRateConfig->rates[axis] = 0; + controlRateConfig->rates[axis] = 50; } } @@ -411,7 +401,7 @@ static void resetConf(void) masterConfig.dcm_ki = 0; // 0.003 * 10000 masterConfig.gyro_lpf = 0; // 256HZ default masterConfig.gyro_sync_denom = 8; - masterConfig.gyro_soft_lpf_hz = 60; + masterConfig.gyro_soft_lpf_hz = 80.0f; masterConfig.pid_process_denom = 1; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 85a74d956..f0733c601 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -58,10 +58,9 @@ int16_t axisPID[3]; int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif -#define DELTA_MAX_SAMPLES 12 - // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; +float tpaFactor; static int32_t errorGyroI[3], errorGyroILimit[3]; static float errorGyroIf[3], errorGyroIfLimit[3]; @@ -107,6 +106,13 @@ void pidResetErrorGyroState(uint8_t resetOption) } } +float getdT (void) { + static float dT; + if (!dT) dT = (float)targetPidLooptime * 0.000001f; + + return dT; +} + void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) { float rcCommandReflection = (float)rcCommand[axis] / 500.0f; static float iTermScaler[3] = {1.0f, 1.0f, 1.0f}; @@ -134,8 +140,8 @@ void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) { const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -static biquad_t deltaBiQuadState[3]; -static bool deltaStateIsSet; +static filterStatePt1_t deltaFilterState[3]; +static filterStatePt1_t yawFilterState; static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) @@ -143,19 +149,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa float RateError, AngleRate, gyroRate; float ITerm,PTerm,DTerm; static float lastErrorForDelta[3]; - static float previousDelta[3][DELTA_MAX_SAMPLES]; - static float previousAverageDelta[3]; - float delta, deltaSum; - int axis, deltaCount; + static float deltaState[3][DELTA_MAX_SAMPLES]; + float delta; + int axis; float horizonLevelStrength = 1; - float dT = (float)targetPidLooptime * 0.000001f; - - if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime); - deltaStateIsSet = true; - } - if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); @@ -191,11 +189,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa #endif if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed - AngleRate = errorAngle * pidProfile->A_level; + AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; } else { // HORIZON mode - direct sticks control is applied to rate PID // mix up angle error to desired AngleRate to add a little auto-level feel - AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; + AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] / 10.0f * horizonLevelStrength; } } } @@ -208,10 +206,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = AngleRate - gyroRate; - if (lowThrottlePidReduction) RateError /= 4; - // -----calculate P component - PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; + PTerm = RateError * (pidProfile->P8[axis] / 40.0f) * tpaFactor; // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { @@ -219,7 +215,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } // -----calculate I component. - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * getdT() * pidProfile->I8[axis] / 10.0f, -250.0f, 250.0f); if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) { if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) scaleItermToRcInput(axis, pidProfile); @@ -245,26 +241,22 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // would be scaled by different dt each time. Division by dT fixes that. - delta *= (1.0f / dT); + delta *= (1.0f / getdT()); - if (deltaStateIsSet) { - delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]); - } else { - // Apply moving average - deltaSum = 0; - for (deltaCount = pidProfile->dterm_average_count-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1]; - previousDelta[axis][0] = delta; - for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount]; - delta = ((deltaSum / pidProfile->dterm_average_count) + previousAverageDelta[axis]) / 2; // Keep same original scaling + double pass averaging - previousAverageDelta[axis] = delta; - } + // Filter delta + if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); - DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); + // Apply moving average + if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]); + + DTerm = constrainf(delta * (float)pidProfile->D8[axis] * 0.001f * tpaFactor, -300.0f, 300.0f); // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); - if (lowThrottlePidReduction) axisPID[axis] /= 4; + if (pidProfile->yaw_lpf_hz && axis == YAW) axisPID[axis] = filterApplyPt1(axisPID[axis], &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); + + if (lowThrottlePidReduction) axisPID[axis] /= 3; #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { @@ -285,17 +277,11 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control { UNUSED(rxConfig); - int axis, deltaCount, prop = 0; + int axis, prop = 0; int32_t rc, error, errorAngle, delta, gyroError; int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm; static int16_t lastErrorForDelta[2]; - static int32_t previousDelta[2][DELTA_MAX_SAMPLES]; - static int32_t previousAverageDelta[2]; - - if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { - for (axis = 0; axis < 2; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime); - deltaStateIsSet = true; - } + static int32_t deltaState[3][DELTA_MAX_SAMPLES]; if (FLIGHT_MODE(HORIZON_MODE)) { prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512); @@ -306,8 +292,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control rc = rcCommand[axis] << 1; - if (lowThrottlePidReduction) rc /= 4; - gyroError = gyroADC[axis] / 4; error = rc - gyroError; @@ -368,23 +352,17 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control // Scale delta to looptime delta = (delta * ((uint16_t) 0xFFFF)) / ((uint16_t)targetPidLooptime << 5); - if (deltaStateIsSet) { - DTerm = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta - } else { - // Apply moving average - DTerm = 0; - for (deltaCount = pidProfile->dterm_average_count-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1]; - previousDelta[axis][0] = delta; - for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) DTerm += previousDelta[axis][deltaCount]; - DTerm = (((DTerm / pidProfile->dterm_average_count) << 1) + previousAverageDelta[axis]) >> 1; // Keep same original scaling + double pass averaging - previousAverageDelta[axis] = DTerm; - } + // Filer delta + if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); - DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation + // Apply moving average and multiply to get original scaling + if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2; + + DTerm = (delta * dynD8[axis]) >> 5; // 32 bits is needed for calculation axisPID[axis] = PTerm + ITerm + DTerm; - if (lowThrottlePidReduction) axisPID[axis] /= 4; + if (lowThrottlePidReduction) axisPID[axis] /= 3; #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { @@ -421,7 +399,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control axisPID[FD_YAW] = PTerm + ITerm; - if (lowThrottlePidReduction) axisPID[FD_YAW] /= 4; + if (pidProfile->yaw_lpf_hz) axisPID[FD_YAW] = filterApplyPt1(axisPID[FD_YAW], &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { @@ -441,20 +419,14 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co { UNUSED(rxConfig); - int axis, deltaCount; - int32_t PTerm, ITerm, DTerm, delta, deltaSum; + int axis; + int32_t PTerm, ITerm, DTerm, delta; static int32_t lastErrorForDelta[3] = { 0, 0, 0 }; - static int32_t previousDelta[3][DELTA_MAX_SAMPLES]; - static int32_t previousAverageDelta[3]; + static int32_t deltaState[3][DELTA_MAX_SAMPLES]; int32_t AngleRateTmp, RateError, gyroRate; int8_t horizonLevelStrength = 100; - if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime); - deltaStateIsSet = true; - } - if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); @@ -504,8 +476,6 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co gyroRate = gyroADC[axis] / 4; RateError = AngleRateTmp - gyroRate; - if (lowThrottlePidReduction) RateError /= 4; - // -----calculate P component PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; @@ -549,24 +519,20 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // would be scaled by different dt each time. Division by dT fixes that. delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 6; - if (deltaStateIsSet) { - delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta - } else { - // Apply moving average - deltaSum = 0; - for (deltaCount = pidProfile->dterm_average_count -1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1]; - previousDelta[axis][0] = delta; - for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount]; - delta = (((deltaSum / pidProfile->dterm_average_count) << 1) + previousAverageDelta[axis]) >> 1; // Keep same original scaling + double pass averaging - previousAverageDelta[axis] = delta; - } + // Filter delta + if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); + + // Apply moving average + if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2; DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; // -----calculate total PID output axisPID[axis] = PTerm + ITerm + DTerm; - if (lowThrottlePidReduction) axisPID[axis] /= 4; + if (pidProfile->yaw_lpf_hz && axis == YAW) axisPID[axis] = filterApplyPt1(axisPID[axis], &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); + + if (lowThrottlePidReduction) axisPID[axis] /= 3; #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 8cd7c3621..50ecaf460 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -63,14 +63,10 @@ typedef struct pidProfile_s { uint8_t I8[PID_ITEM_COUNT]; uint8_t D8[PID_ITEM_COUNT]; - float P_f[3]; // float p i and d factors for lux float pid controller - float I_f[3]; - float D_f[3]; - float A_level; - float H_level; uint8_t H_sensitivity; float dterm_lpf_hz; // Delta Filter in hz + float yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy uint8_t deltaMethod; // Alternative delta Calculation uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm diff --git a/src/main/io/i2c_bst.c b/src/main/io/i2c_bst.c index 453c9c8c5..f0891e8e0 100644 --- a/src/main/io/i2c_bst.c +++ b/src/main/io/i2c_bst.c @@ -700,29 +700,10 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8(currentControlRateProfile->rcYawExpo8); break; case BST_PID: - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid - for (i = 0; i < 3; i++) { - bstWrite8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250)); - bstWrite8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250)); - bstWrite8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 1000.0f), 0, 100)); - } - for (i = 3; i < PID_ITEM_COUNT; i++) { - if (i == PIDLEVEL) { - bstWrite8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250)); - bstWrite8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250)); - bstWrite8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 250)); - } else { - bstWrite8(currentProfile->pidProfile.P8[i]); - bstWrite8(currentProfile->pidProfile.I8[i]); - bstWrite8(currentProfile->pidProfile.D8[i]); - } - } - } else { - for (i = 0; i < PID_ITEM_COUNT; i++) { - bstWrite8(currentProfile->pidProfile.P8[i]); - bstWrite8(currentProfile->pidProfile.I8[i]); - bstWrite8(currentProfile->pidProfile.D8[i]); - } + for (i = 0; i < PID_ITEM_COUNT; i++) { + bstWrite8(currentProfile->pidProfile.P8[i]); + bstWrite8(currentProfile->pidProfile.I8[i]); + bstWrite8(currentProfile->pidProfile.D8[i]); } break; case BST_PIDNAMES: @@ -1066,30 +1047,11 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) pidSetController(currentProfile->pidProfile.pidController); break; case BST_SET_PID: - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { - for (i = 0; i < 3; i++) { - currentProfile->pidProfile.P_f[i] = (float)bstRead8() / 10.0f; - currentProfile->pidProfile.I_f[i] = (float)bstRead8() / 100.0f; - currentProfile->pidProfile.D_f[i] = (float)bstRead8() / 1000.0f; - } - for (i = 3; i < PID_ITEM_COUNT; i++) { - if (i == PIDLEVEL) { - currentProfile->pidProfile.A_level = (float)bstRead8() / 10.0f; - currentProfile->pidProfile.H_level = (float)bstRead8() / 10.0f; - currentProfile->pidProfile.H_sensitivity = bstRead8(); - } else { - currentProfile->pidProfile.P8[i] = bstRead8(); - currentProfile->pidProfile.I8[i] = bstRead8(); - currentProfile->pidProfile.D8[i] = bstRead8(); - } - } - } else { - for (i = 0; i < PID_ITEM_COUNT; i++) { - currentProfile->pidProfile.P8[i] = bstRead8(); - currentProfile->pidProfile.I8[i] = bstRead8(); - currentProfile->pidProfile.D8[i] = bstRead8(); - } - } + for (i = 0; i < PID_ITEM_COUNT; i++) { + currentProfile->pidProfile.P8[i] = bstRead8(); + currentProfile->pidProfile.I8[i] = bstRead8(); + currentProfile->pidProfile.D8[i] = bstRead8(); + } break; case BST_SET_MODE_RANGE: i = bstRead8(); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 2441bf857..4b6f8204f 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -476,7 +476,6 @@ void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adj void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { int newValue; - float newFloatValue; if (delta > 0) { beeperConfirmationBeeps(2); @@ -523,114 +522,63 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_P: case ADJUSTMENT_PITCH_P: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->P_f[PIDPITCH] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->P_f[PIDPITCH] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_P, newFloatValue); - } else { - newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->P8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); - } + newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); + if (adjustmentFunction == ADJUSTMENT_PITCH_P) { break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_P case ADJUSTMENT_ROLL_P: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->P_f[PIDROLL] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->P_f[PIDROLL] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_P, newFloatValue); - } else { - newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->P8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); - } + newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); break; case ADJUSTMENT_PITCH_ROLL_I: case ADJUSTMENT_PITCH_I: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->I_f[PIDPITCH] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_I, newFloatValue); - } else { - newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->I8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); - } + newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); + if (adjustmentFunction == ADJUSTMENT_PITCH_I) { break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_I case ADJUSTMENT_ROLL_I: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->I_f[PIDROLL] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_I, newFloatValue); - } else { - newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->I8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); - } + newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); break; case ADJUSTMENT_PITCH_ROLL_D: case ADJUSTMENT_PITCH_D: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->D_f[PIDPITCH] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_D, newFloatValue); - } else { - newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->D8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); - } + newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); + if (adjustmentFunction == ADJUSTMENT_PITCH_D) { break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_D case ADJUSTMENT_ROLL_D: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->D_f[PIDROLL] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_D, newFloatValue); - } else { - newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->D8[PIDROLL] = newValue; + newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D8[PIDROLL] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); - } break; case ADJUSTMENT_YAW_P: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->P_f[PIDYAW] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->P_f[PIDYAW] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_P, newFloatValue); - } else { - newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->P8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); - } + newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); break; case ADJUSTMENT_YAW_I: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->I_f[PIDYAW] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_I, newFloatValue); - } else { - newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->I8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); - } + newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); break; case ADJUSTMENT_YAW_D: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->D_f[PIDYAW] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_D, newFloatValue); - } else { - newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->D8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); - } + newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); break; default: break; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5cc4bac69..b38bef3f2 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -728,7 +728,8 @@ const clivalue_t valueTable[] = { { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, - { "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {2, 12 } }, + { "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } }, + { "yaw_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, @@ -743,18 +744,6 @@ const clivalue_t valueTable[] = { { "i_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[YAW], .config.minmax = { 0, 200 } }, { "d_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[YAW], .config.minmax = { 0, 200 } }, - { "p_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[PITCH], .config.minmax = { 0, 100 } }, - { "i_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[PITCH], .config.minmax = { 0, 100 } }, - { "d_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[PITCH], .config.minmax = { 0, 100 } }, - { "p_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[ROLL], .config.minmax = { 0, 100 } }, - { "i_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[ROLL], .config.minmax = { 0, 100 } }, - { "d_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[ROLL], .config.minmax = { 0, 100 } }, - { "p_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[YAW], .config.minmax = { 0, 100 } }, - { "i_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[YAW], .config.minmax = { 0, 100 } }, - { "d_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[YAW], .config.minmax = { 0, 100 } }, - - { "level_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, .config.minmax = { 0, 10 } }, - { "level_angle", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, .config.minmax = { 0, 10 } }, { "sensitivity_horizon", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, .config.minmax = { 0, 250 } }, { "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], .config.minmax = { 0, 200 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 3e9742e8d..1ae8862ec 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -879,29 +879,10 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid - for (i = 0; i < 3; i++) { - serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 40.0f), 0, 255)); - serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 255)); - serialize8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 4000.0f), 0, 255)); - } - for (i = 3; i < PID_ITEM_COUNT; i++) { - if (i == PIDLEVEL) { - serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 255)); - serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 255)); - serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 255)); - } else { - serialize8(currentProfile->pidProfile.P8[i]); - serialize8(currentProfile->pidProfile.I8[i]); - serialize8(currentProfile->pidProfile.D8[i]); - } - } - } else { - for (i = 0; i < PID_ITEM_COUNT; i++) { - serialize8(currentProfile->pidProfile.P8[i]); - serialize8(currentProfile->pidProfile.I8[i]); - serialize8(currentProfile->pidProfile.D8[i]); - } + for (i = 0; i < PID_ITEM_COUNT; i++) { + serialize8(currentProfile->pidProfile.P8[i]); + serialize8(currentProfile->pidProfile.I8[i]); + serialize8(currentProfile->pidProfile.D8[i]); } break; case MSP_PIDNAMES: @@ -1335,29 +1316,10 @@ static bool processInCommand(void) if (oldPid != currentProfile->pidProfile.pidController) setGyroSamplingSpeed(0); // recalculate looptimes for new PID break; case MSP_SET_PID: - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { - for (i = 0; i < 3; i++) { - currentProfile->pidProfile.P_f[i] = (float)read8() / 40.0f; - currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f; - currentProfile->pidProfile.D_f[i] = (float)read8() / 4000.0f; - } - for (i = 3; i < PID_ITEM_COUNT; i++) { - if (i == PIDLEVEL) { - currentProfile->pidProfile.A_level = (float)read8() / 10.0f; - currentProfile->pidProfile.H_level = (float)read8() / 10.0f; - currentProfile->pidProfile.H_sensitivity = read8(); - } else { - currentProfile->pidProfile.P8[i] = read8(); - currentProfile->pidProfile.I8[i] = read8(); - currentProfile->pidProfile.D8[i] = read8(); - } - } - } else { - for (i = 0; i < PID_ITEM_COUNT; i++) { - currentProfile->pidProfile.P8[i] = read8(); - currentProfile->pidProfile.I8[i] = read8(); - currentProfile->pidProfile.D8[i] = read8(); - } + for (i = 0; i < PID_ITEM_COUNT; i++) { + currentProfile->pidProfile.P8[i] = read8(); + currentProfile->pidProfile.I8[i] = read8(); + currentProfile->pidProfile.D8[i] = read8(); } break; case MSP_SET_MODE_RANGE: diff --git a/src/main/mw.c b/src/main/mw.c index 1edd6441e..d079047f3 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -103,8 +103,6 @@ enum { uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop -float dT; - int16_t magHold; int16_t headFreeModeHold; @@ -115,6 +113,7 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m extern uint32_t currentTime; extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; +extern float tpaFactor; extern bool antiWindupProtection; uint16_t filteredCycleTime; @@ -234,6 +233,7 @@ void annexCode(void) prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); } else { prop2 = 100 - currentControlRateProfile->dynThrPID; + tpaFactor = prop2 / 100.0f; } } diff --git a/src/main/version.h b/src/main/version.h index 65e7dc436..c79b4db7a 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -16,8 +16,8 @@ */ #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 5 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 4 // increment when a bug is fixed +#define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) From 595d5d0867976a277ef80211ee77113d7ae35a94 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Mar 2016 12:38:29 +0200 Subject: [PATCH 06/78] Acro Plus replaced by Super Expo feature --- src/main/config/config.c | 5 +-- src/main/flight/mixer.c | 33 ------------------ src/main/flight/pid.c | 70 +++++++++++++++++++++------------------ src/main/flight/pid.h | 4 ++- src/main/io/rc_controls.h | 2 +- src/main/io/serial_cli.c | 5 +-- src/main/io/serial_msp.c | 6 ++-- src/main/rx/rx.h | 3 +- 8 files changed, 51 insertions(+), 77 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 846755ff5..d22e5bfe0 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -179,6 +179,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 0.0f; pidProfile->dterm_average_count = 4; + pidProfile->rollPitchItermResetRate = 0; + pidProfile->yawItermResetRate = 0; pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; @@ -450,8 +452,7 @@ static void resetConf(void) masterConfig.rxConfig.rcSmoothing = 0; masterConfig.rxConfig.fpvCamAngleDegrees = 0; masterConfig.rxConfig.max_aux_channel = 6; - masterConfig.rxConfig.acroPlusFactor = 30; - masterConfig.rxConfig.acroPlusOffset = 40; + masterConfig.rxConfig.superExpoFactor = 20; resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index f61c068ee..802e7e780 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -748,35 +748,6 @@ STATIC_UNIT_TESTED void servoMixer(void) #endif -void acroPlusApply(void) { - int axis; - - for (axis = 0; axis < 2; axis++) { - int16_t factor; - fix12_t wowFactor; - int16_t rcCommandDeflection = constrain(rcCommand[axis], -500, 500); // Limit stick input to 500 (rcCommand 100) - int16_t acroPlusStickOffset = rxConfig->acroPlusOffset * 5; - int16_t motorRange = escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle; - if (feature(FEATURE_3D)) motorRange = (motorRange - (flight3DConfig->deadband3d_high - flight3DConfig->deadband3d_low)) / 2; - - /* acro plus factor handling */ - if (rxConfig->acroPlusFactor && ABS(rcCommandDeflection) > acroPlusStickOffset + 10) { - if (rcCommandDeflection > 0) { - rcCommandDeflection -= acroPlusStickOffset; - } else { - rcCommandDeflection += acroPlusStickOffset; - } - wowFactor = qConstruct(ABS(rcCommandDeflection) * rxConfig->acroPlusFactor / 100, 500); - factor = qMultiply(wowFactor, (rcCommandDeflection * motorRange) / 500); - wowFactor = Q12 - wowFactor; - } else { - wowFactor = Q12; - factor = 0; - } - axisPID[axis] = factor + qMultiply(wowFactor, axisPID[axis]); - } -} - void mixTable(void) { uint32_t i; @@ -785,10 +756,6 @@ void mixTable(void) bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code - if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) { - acroPlusApply(); - } - if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) { // prevent "yaw jump" during yaw correction axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW])); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f0733c601..76fcc580f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -80,6 +80,14 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) { targetPidLooptime = targetLooptime * pidProcessDenom; } +float calculateExpoPlus(int axis, rxConfig_t *rxConfig) { + float propFactor; + + propFactor = 1.0f - ((rxConfig->superExpoFactor / 100.0f) * (ABS(rcCommand[axis]) / 500.0f)); + + return propFactor; +} + void pidResetErrorAngle(void) { errorAngleI[ROLL] = 0; @@ -113,31 +121,6 @@ float getdT (void) { return dT; } -void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) { - float rcCommandReflection = (float)rcCommand[axis] / 500.0f; - static float iTermScaler[3] = {1.0f, 1.0f, 1.0f}; - static float antiWindUpIncrement = 0; - - if (!antiWindUpIncrement) antiWindUpIncrement = (0.001 / 500) * targetPidLooptime; // Calculate increment for 500ms period - - if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */ - /* Reset Iterm on high stick inputs. No scaling necessary here */ - iTermScaler[axis] = 0.0f; - errorGyroI[axis] = 0; - errorGyroIf[axis] = 0.0f; - } else { - /* Prevent rapid windup during acro recoveries. Slowly enable Iterm for period of 500ms */ - if (iTermScaler[axis] < 1) { - iTermScaler[axis] = constrainf(iTermScaler[axis] + antiWindUpIncrement, 0.0f, 1.0f); - if (pidProfile->pidController != PID_CONTROLLER_LUX_FLOAT) { - errorGyroI[axis] *= iTermScaler[axis]; - } else { - errorGyroIf[axis] *= iTermScaler[axis]; - } - } - } -} - const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; static filterStatePt1_t deltaFilterState[3]; @@ -207,7 +190,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa RateError = AngleRate - gyroRate; // -----calculate P component - PTerm = RateError * (pidProfile->P8[axis] / 40.0f) * tpaFactor; + if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) { + PTerm = (pidProfile->P8[axis] / 40.0f * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig)); + } else { + PTerm = RateError * (pidProfile->P8[axis] / 40.0f) * tpaFactor; + } // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { @@ -217,8 +204,15 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate I component. errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * getdT() * pidProfile->I8[axis] / 10.0f, -250.0f, 250.0f); - if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) { - if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) scaleItermToRcInput(axis, pidProfile); + if (pidProfile->rollPitchItermResetRate && axis != YAW) { + if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; + } + + if (pidProfile->yawItermResetRate && axis == YAW) { + if (ABS(gyroRate) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; + } + + if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { if (antiWindupProtection || motorLimitReached) { errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]); } else { @@ -302,8 +296,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control } // Anti windup protection - if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) { - if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) scaleItermToRcInput(axis, pidProfile); + if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { if (antiWindupProtection || motorLimitReached) { errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); } else { @@ -477,7 +470,11 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co RateError = AngleRateTmp - gyroRate; // -----calculate P component - PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; + if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) { + PTerm = (pidProfile->P8[axis] * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7; + } else { + PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; + } // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { @@ -495,8 +492,15 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // I coefficient (I8) moved before integration to make limiting independent from PID settings errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); - if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) { - if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) scaleItermToRcInput(axis, pidProfile); + if (pidProfile->rollPitchItermResetRate && axis != YAW) { + if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; + } + + if (pidProfile->yawItermResetRate && axis == YAW) { + if (ABS(gyroRate / 4) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; + } + + if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { if (antiWindupProtection || motorLimitReached) { errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); } else { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 50ecaf460..857138381 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -67,7 +67,9 @@ typedef struct pidProfile_s { float dterm_lpf_hz; // Delta Filter in hz float yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy - uint8_t deltaMethod; // Alternative delta Calculation + uint8_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates + uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates + uint16_t deltaMethod; // Alternative delta Calculation uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 9674e3580..7756aed07 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -49,7 +49,7 @@ typedef enum { BOXBLACKBOX, BOXFAILSAFE, BOXAIRMODE, - BOXACROPLUS, + BOXSUPEREXPO, BOX3DDISABLESWITCH, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b38bef3f2..ba80c4e09 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -693,8 +693,7 @@ const clivalue_t valueTable[] = { { "yaw_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, - { "acro_plus_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusFactor, .config.minmax = {1, 100 } }, - { "acro_plus_offset", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusOffset, .config.minmax = {1, 90 } }, + { "super_expo_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactor, .config.minmax = {1, 100 } }, { "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0, 200 } }, { "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_off_delay, .config.minmax = { 0, 200 } }, @@ -729,6 +728,8 @@ const clivalue_t valueTable[] = { { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } }, + { "roll_pitch_iterm_reset_deg", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {0, 1000 } }, + { "yaw_iterm_reset_deg", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {0, 1000 } }, { "yaw_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 1ae8862ec..8c42ad3c5 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -219,7 +219,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXBLACKBOX, "BLACKBOX;", 26 }, { BOXFAILSAFE, "FAILSAFE;", 27 }, { BOXAIRMODE, "AIR MODE;", 28 }, - { BOXACROPLUS, "ACRO PLUS;", 29 }, + { BOXSUPEREXPO, "SUPER EXPO;", 29 }, { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 30}, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -547,7 +547,7 @@ void mspInit(serialConfig_t *serialConfig) } activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; - activeBoxIds[activeBoxIdCount++] = BOXACROPLUS; + activeBoxIds[activeBoxIdCount++] = BOXSUPEREXPO; activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH; if (sensors(SENSOR_BARO)) { @@ -654,7 +654,7 @@ static uint32_t packFlightModeFlags(void) IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXACROPLUS)) << BOXACROPLUS; + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) << BOXSUPEREXPO; for (i = 0; i < activeBoxIdCount; i++) { int flag = (tmp & (1 << activeBoxIds[i])); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 1d546554c..121f0d636 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -124,8 +124,7 @@ typedef struct rxConfig_s { uint8_t rcSmoothing; uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands uint8_t max_aux_channel; - uint8_t acroPlusFactor; // Air mode acrobility factor - uint8_t acroPlusOffset; // Air mode stick offset + uint8_t superExpoFactor; // Super Expo Factor uint16_t rx_min_usec; uint16_t rx_max_usec; From bdca96842afcd883ddff81f01807c01a0161c913 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Mar 2016 16:40:55 +0200 Subject: [PATCH 07/78] Add dump all cli command and restore old dump --- src/main/io/serial_cli.c | 46 ++++++++++++++++++++++++---------------- 1 file changed, 28 insertions(+), 18 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ba80c4e09..f21b165d6 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1787,6 +1787,7 @@ typedef enum { DUMP_MASTER = (1 << 0), DUMP_PROFILE = (1 << 1), DUMP_RATES = (1 << 2), + DUMP_ALL = (1 << 3), } dumpFlags_e; @@ -1815,7 +1816,11 @@ static void cliDump(char *cmdline) dumpMask = DUMP_RATES; } - if (dumpMask & DUMP_MASTER) { + if (strcasecmp(cmdline, "all") == 0) { + dumpMask = DUMP_ALL; // All profiles and rates + } + + if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { cliPrint("\r\n# version\r\n"); cliVersion(NULL); @@ -1957,22 +1962,32 @@ static void cliDump(char *cmdline) cliPrint("\r\n# rxfail\r\n"); cliRxFail(""); - uint8_t activeProfile = masterConfig.current_profile_index; - uint8_t i; - for (i=0; iactiveRateProfile; + uint8_t profileCount; + uint8_t rateCount; + for (profileCount=0; profileCountactiveRateProfile); + } } if (dumpMask & DUMP_PROFILE) { cliDumpProfile(masterConfig.current_profile_index); - } + if (dumpMask & DUMP_RATES) { cliDumpRateProfile(currentProfile->activeRateProfile); - } + } } @@ -1983,16 +1998,11 @@ void cliDumpProfile(uint8_t profileIndex) { changeProfile(profileIndex); cliPrint("\r\n# profile\r\n"); cliProfile(""); - cliPrintf("################################################################################"); + cliPrintf("############################# PROFILE VALUES ####################################"); + cliProfile(""); printSectionBreak(); dumpValues(PROFILE_VALUE); - uint8_t currentRateIndex = currentProfile->activeRateProfile; - uint8_t i; - for (i=0; i Date: Wed, 30 Mar 2016 16:43:01 +0200 Subject: [PATCH 08/78] Change defaults --- src/main/config/config.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index d22e5bfe0..a715193ec 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -145,15 +145,15 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) static void resetPidProfile(pidProfile_t *pidProfile) { - pidProfile->pidController = 2; + pidProfile->pidController = 1; - pidProfile->P8[ROLL] = 40; + pidProfile->P8[ROLL] = 45; pidProfile->I8[ROLL] = 30; pidProfile->D8[ROLL] = 18; - pidProfile->P8[PITCH] = 40; + pidProfile->P8[PITCH] = 45; pidProfile->I8[PITCH] = 30; pidProfile->D8[PITCH] = 18; - pidProfile->P8[YAW] = 60; + pidProfile->P8[YAW] = 90; pidProfile->I8[YAW] = 40; pidProfile->D8[YAW] = 0; pidProfile->P8[PIDALT] = 50; @@ -172,9 +172,9 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->I8[PIDLEVEL] = 50; pidProfile->D8[PIDLEVEL] = 100; pidProfile->P8[PIDMAG] = 40; - pidProfile->P8[PIDVEL] = 120; - pidProfile->I8[PIDVEL] = 45; - pidProfile->D8[PIDVEL] = 1; + pidProfile->P8[PIDVEL] = 55; + pidProfile->I8[PIDVEL] = 55; + pidProfile->D8[PIDVEL] = 0; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 0.0f; From cbc7bc2a61b7b2e0f1232f72e422c03c5c348fda Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Mar 2016 16:54:33 +0200 Subject: [PATCH 09/78] Increase PID performance by removing Dterm for Yaw --- src/main/flight/pid.c | 82 +++++++++++++++++++++++-------------------- 1 file changed, 44 insertions(+), 38 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 76fcc580f..08470d5b1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -225,31 +225,34 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa ITerm = errorGyroIf[axis]; //-----calculate D-term - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateError - lastErrorForDelta[axis]; - lastErrorForDelta[axis] = RateError; + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); + DTerm = 0; } else { - delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastErrorForDelta[axis] = gyroRate; + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + delta = RateError - lastErrorForDelta[axis]; + lastErrorForDelta[axis] = RateError; + } else { + delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastErrorForDelta[axis] = gyroRate; + } + + // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference + // would be scaled by different dt each time. Division by dT fixes that. + delta *= (1.0f / getdT()); + + // Filter delta + if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); + + // Apply moving average + if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]); + + DTerm = constrainf(delta * (float)pidProfile->D8[axis] * 0.001f * tpaFactor, -300.0f, 300.0f); } - // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference - // would be scaled by different dt each time. Division by dT fixes that. - delta *= (1.0f / getdT()); - - // Filter delta - if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); - - // Apply moving average - if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]); - - DTerm = constrainf(delta * (float)pidProfile->D8[axis] * 0.001f * tpaFactor, -300.0f, 300.0f); - // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); - if (pidProfile->yaw_lpf_hz && axis == YAW) axisPID[axis] = filterApplyPt1(axisPID[axis], &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); - if (lowThrottlePidReduction) axisPID[axis] /= 3; #ifdef GTUNE @@ -511,31 +514,34 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co ITerm = errorGyroI[axis] >> 13; //-----calculate D-term - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateError - lastErrorForDelta[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastErrorForDelta[axis] = RateError; + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); + DTerm = 0; } else { - delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastErrorForDelta[axis] = gyroRate; + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + delta = RateError - lastErrorForDelta[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastErrorForDelta[axis] = RateError; + } else { + delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastErrorForDelta[axis] = gyroRate; + } + + // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference + // would be scaled by different dt each time. Division by dT fixes that. + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 6; + + // Filter delta + if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); + + // Apply moving average + if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2; + + DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; } - - // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference - // would be scaled by different dt each time. Division by dT fixes that. - delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 6; - - // Filter delta - if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); - - // Apply moving average - if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2; - - DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; // -----calculate total PID output axisPID[axis] = PTerm + ITerm + DTerm; - if (pidProfile->yaw_lpf_hz && axis == YAW) axisPID[axis] = filterApplyPt1(axisPID[axis], &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); - if (lowThrottlePidReduction) axisPID[axis] /= 3; #ifdef GTUNE From 95a464b1ad9b70c5b8fe6dbf9712fb908e75eb36 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Mar 2016 17:13:59 +0200 Subject: [PATCH 10/78] Add escpassthrough to all targets --- Makefile | 2 +- src/main/io/serial_cli.c | 1 + src/main/io/serial_msp.c | 1 + src/main/target/ALIENFLIGHTF3/target.h | 3 +++ src/main/target/CC3D/target.h | 2 ++ src/main/target/IRCFUSIONF3/target.h | 3 +++ src/main/target/LUX_RACE/target.h | 3 +++ src/main/target/MOTOLAB/target.h | 3 +++ src/main/target/NAZE/target.h | 3 +++ src/main/target/RMDO/target.h | 3 +++ src/main/target/SPARKY/target.h | 3 +++ src/main/target/SPRACINGF3/target.h | 2 ++ src/main/target/SPRACINGF3MINI/target.h | 2 ++ 13 files changed, 30 insertions(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 384b03035..f0234cc2b 100644 --- a/Makefile +++ b/Makefile @@ -299,6 +299,7 @@ COMMON_SRC = build_config.c \ drivers/gyro_sync.c \ drivers/dma.c \ drivers/buf_writer.c \ + drivers/serial_escserial.c \ io/beeper.c \ io/rc_controls.c \ io/rc_curves.c \ @@ -604,7 +605,6 @@ 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/io/serial_cli.c b/src/main/io/serial_cli.c index f21b165d6..1e178c10d 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -47,6 +47,7 @@ #include "drivers/pwm_rx.h" #include "drivers/sdcard.h" #include "drivers/gyro_sync.h" +#include "drivers/serial_escserial.h" #include "drivers/buf_writer.h" diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 8c42ad3c5..bd8ee07a5 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -44,6 +44,7 @@ #include "drivers/gyro_sync.h" #include "drivers/sdcard.h" #include "drivers/buf_writer.h" +#include "drivers/serial_escserial.h" #include "rx/rx.h" #include "rx/msp.h" diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 79365c62e..ca89307f7 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -51,6 +51,9 @@ //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + // Using MPU6050 for the moment. #define GYRO #define USE_GYRO_MPU6050 diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 486463ac9..e57a64270 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -27,6 +27,8 @@ #define INVERTER_PERIPHERAL RCC_APB2Periph_GPIOB #define INVERTER_USART USART1 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define BEEP_GPIO GPIOA #define BEEP_PIN Pin_15 // PA15 (Beeper) diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 7f288779f..4152bbfb0 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -38,6 +38,9 @@ #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define GYRO #define USE_GYRO_MPU6050 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index a184c1b46..df441c319 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -58,6 +58,9 @@ #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 9a9eedc50..f49661f85 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -35,6 +35,9 @@ #define USABLE_TIMER_CHANNEL_COUNT 9 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + // MPU6050 interrupts #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 0a09a7bfa..7ec9848ae 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -29,6 +29,9 @@ #define LED1_PIN Pin_4 // PB4 (LED) #define LED1_PERIPHERAL RCC_APB2Periph_GPIOB +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define BEEP_GPIO GPIOA #define BEEP_PIN Pin_12 // PA12 (Beeper) #define BEEP_PERIPHERAL RCC_APB2Periph_GPIOA diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index eca7c340d..64e5f1246 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -28,6 +28,9 @@ #define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC #define BEEPER_INVERTED +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define USABLE_TIMER_CHANNEL_COUNT 17 #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 2c94162b5..907a3cb09 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -34,6 +34,9 @@ #define USABLE_TIMER_CHANNEL_COUNT 11 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + // MPU6050 interrupts #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 822473821..bd4c60bba 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -38,6 +38,8 @@ #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define GYRO #define USE_GYRO_MPU6050 diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index b2fc73941..0fd8b2232 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -41,6 +41,8 @@ #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define GYRO //#define USE_FAKE_GYRO From 78c155908a9292cf999e6dc73eced3ecb0c1e97c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Mar 2016 17:15:42 +0200 Subject: [PATCH 11/78] Bump EEPROM --- src/main/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index a715193ec..893b451ae 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 130; +static const uint8_t EEPROM_CONF_VERSION = 131; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { From 712beb2ae85862fc82ec82542f47f4f4e3357820 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Mar 2016 17:29:12 +0200 Subject: [PATCH 12/78] Prevent Arming before gyro calibration completed --- src/main/mw.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index d079047f3..b84abf50d 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -368,9 +368,11 @@ void releaseSharedTelemetryPorts(void) { void mwArm(void) { - if (!ARMING_FLAG(WAS_EVER_ARMED) && masterConfig.gyro_cal_on_first_arm) { + if (!ARMING_FLAG(WAS_EVER_ARMED) && masterConfig.gyro_cal_on_first_arm) { gyroSetCalibrationCycles(calculateCalibratingCycles()); - } + } + + if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated if (ARMING_FLAG(OK_TO_ARM)) { if (ARMING_FLAG(ARMED)) { From 3e5e30c42ca84535eddcc0253527a6fa74685a5f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Mar 2016 21:23:14 +0200 Subject: [PATCH 13/78] Fix Iterm reset in super expo mode // new defaults --- src/main/config/config.c | 4 +-- src/main/flight/pid.c | 54 ++++++++++++++++++---------------------- src/main/flight/pid.h | 4 +-- src/main/io/serial_cli.c | 4 +-- 4 files changed, 30 insertions(+), 36 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 893b451ae..9b56e39b0 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -179,8 +179,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 0.0f; pidProfile->dterm_average_count = 4; - pidProfile->rollPitchItermResetRate = 0; - pidProfile->yawItermResetRate = 0; + pidProfile->rollPitchItermResetRate = 200; + pidProfile->yawItermResetRate = 50; pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 08470d5b1..65907ec00 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -204,22 +204,20 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate I component. errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * getdT() * pidProfile->I8[axis] / 10.0f, -250.0f, 250.0f); - if (pidProfile->rollPitchItermResetRate && axis != YAW) { - if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; - } - - if (pidProfile->yawItermResetRate && axis == YAW) { - if (ABS(gyroRate) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; - } - - if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { - if (antiWindupProtection || motorLimitReached) { - errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]); + if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) { + if (axis == YAW) { + if (ABS(gyroRate) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; } else { - errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]); + if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; } } + if (antiWindupProtection || motorLimitReached) { + errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]); + } else { + errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]); + } + // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings ITerm = errorGyroIf[axis]; @@ -299,12 +297,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control } // Anti windup protection - if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { - if (antiWindupProtection || motorLimitReached) { - errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); - } else { - errorGyroILimit[axis] = ABS(errorGyroI[axis]); - } + if (antiWindupProtection || motorLimitReached) { + errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); + } else { + errorGyroILimit[axis] = ABS(errorGyroI[axis]); } ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 @@ -495,22 +491,20 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // I coefficient (I8) moved before integration to make limiting independent from PID settings errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); - if (pidProfile->rollPitchItermResetRate && axis != YAW) { - if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; - } - - if (pidProfile->yawItermResetRate && axis == YAW) { - if (ABS(gyroRate / 4) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; - } - - if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { - if (antiWindupProtection || motorLimitReached) { - errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); + if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) { + if (axis == YAW) { + if (ABS(gyroRate / 4) >= pidProfile->yawItermResetRate) errorGyroI[axis] = 0; } else { - errorGyroILimit[axis] = ABS(errorGyroI[axis]); + if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = 0; } } + if (antiWindupProtection || motorLimitReached) { + errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); + } else { + errorGyroILimit[axis] = ABS(errorGyroI[axis]); + } + ITerm = errorGyroI[axis] >> 13; //-----calculate D-term diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 857138381..a30fdfdf1 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -67,9 +67,9 @@ typedef struct pidProfile_s { float dterm_lpf_hz; // Delta Filter in hz float yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy - uint8_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates + uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates - uint16_t deltaMethod; // Alternative delta Calculation + uint8_t deltaMethod; // Alternative delta Calculation uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 1e178c10d..f5fac205a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -729,8 +729,8 @@ const clivalue_t valueTable[] = { { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } }, - { "roll_pitch_iterm_reset_deg", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {0, 1000 } }, - { "yaw_iterm_reset_deg", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {0, 1000 } }, + { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, + { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, { "yaw_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, From 940d666fb02bb1bcf45fc8ecdf20a1e064ad27b3 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Mar 2016 21:26:45 +0200 Subject: [PATCH 14/78] default yaw filter --- src/main/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 9b56e39b0..351dc37a1 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -177,7 +177,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[PIDVEL] = 0; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; - pidProfile->yaw_lpf_hz = 0.0f; + pidProfile->yaw_lpf_hz = 70.0f; pidProfile->dterm_average_count = 4; pidProfile->rollPitchItermResetRate = 200; pidProfile->yawItermResetRate = 50; From f2f1e2c513dde762cf24a49f2d892744a6a48d72 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Mar 2016 21:34:28 +0200 Subject: [PATCH 15/78] Change default expo factor and yaw pterm max --- src/main/config/config.c | 2 +- src/main/flight/pid.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 351dc37a1..6b11222b9 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -452,7 +452,7 @@ static void resetConf(void) masterConfig.rxConfig.rcSmoothing = 0; masterConfig.rxConfig.fpvCamAngleDegrees = 0; masterConfig.rxConfig.max_aux_channel = 6; - masterConfig.rxConfig.superExpoFactor = 20; + masterConfig.rxConfig.superExpoFactor = 30; resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index a30fdfdf1..4208ad6c4 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -20,7 +20,7 @@ #define GYRO_I_MAX 256 // Gyro I limiter #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter -#define YAW_P_LIMIT_MAX 300 // Maximum value for yaw P limiter +#define YAW_P_LIMIT_MAX 400 // Maximum value for yaw P limiter typedef enum { PIDROLL, From ad0acf3f5f9025a0d4963bfaeee243685c0fcd95 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Mar 2016 21:43:41 +0200 Subject: [PATCH 16/78] Change cli naming conventions --- src/main/io/serial_cli.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f5fac205a..e5ac4c5de 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -660,7 +660,7 @@ const clivalue_t valueTable[] = { { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, - { "gyro_soft_lpf", VAR_FLOAT | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } }, + { "gyro_lowpass_hz", VAR_FLOAT | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } }, @@ -727,11 +727,11 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, - { "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, + { "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } }, { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, - { "yaw_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, + { "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, From 268920840898cb8524c68ffd80eab4b38c2a66e3 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 31 Mar 2016 00:26:32 +0200 Subject: [PATCH 17/78] Fix floating point tpa bug --- src/main/config/config.c | 2 +- src/main/flight/pid.c | 3 ++- src/main/mw.c | 2 -- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 6b11222b9..999448218 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -305,7 +305,7 @@ void resetSerialConfig(serialConfig_t *serialConfig) static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcRate8 = 100; - controlRateConfig->rcExpo8 = 20; + controlRateConfig->rcExpo8 = 60; controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; controlRateConfig->dynThrPID = 0; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 65907ec00..d79dec104 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -60,7 +60,6 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; -float tpaFactor; static int32_t errorGyroI[3], errorGyroILimit[3]; static float errorGyroIf[3], errorGyroIfLimit[3]; @@ -137,6 +136,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa int axis; float horizonLevelStrength = 1; + float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float + if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); diff --git a/src/main/mw.c b/src/main/mw.c index b84abf50d..79810bbd4 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -113,7 +113,6 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m extern uint32_t currentTime; extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; -extern float tpaFactor; extern bool antiWindupProtection; uint16_t filteredCycleTime; @@ -233,7 +232,6 @@ void annexCode(void) prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); } else { prop2 = 100 - currentControlRateProfile->dynThrPID; - tpaFactor = prop2 / 100.0f; } } From 4c1b4a63beaed4df8296f164eac6e7b185916cf6 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 31 Mar 2016 00:49:42 +0200 Subject: [PATCH 18/78] Add new line --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index e5ac4c5de..9aba1bd3b 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1999,7 +1999,7 @@ void cliDumpProfile(uint8_t profileIndex) { changeProfile(profileIndex); cliPrint("\r\n# profile\r\n"); cliProfile(""); - cliPrintf("############################# PROFILE VALUES ####################################"); + cliPrintf("############################# PROFILE VALUES ####################################\r\n"); cliProfile(""); printSectionBreak(); dumpValues(PROFILE_VALUE); From f2cb5369c8bd869a8181fddf47479548fee61e25 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 31 Mar 2016 12:40:09 +0200 Subject: [PATCH 19/78] Increase Yaw rate offset rewrite to be closer to roll/pitch --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d79dec104..2da94c25d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -439,7 +439,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // -----Get the desired angle rate depending on flight mode if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) - AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[YAW]) >> 5; + AngleRateTmp = ((int32_t)(rate + 47) * rcCommand[YAW]) >> 5; } else { AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4; if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { From 5272426a00a4b56301a6332928823184b00f0f36 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 31 Mar 2016 12:47:26 +0200 Subject: [PATCH 20/78] Proper fix for calibration prior to arming --- src/main/mw.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index 79810bbd4..f795c7b02 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -366,8 +366,11 @@ void releaseSharedTelemetryPorts(void) { void mwArm(void) { - if (!ARMING_FLAG(WAS_EVER_ARMED) && masterConfig.gyro_cal_on_first_arm) { - gyroSetCalibrationCycles(calculateCalibratingCycles()); + static bool armingCalibrationWasInitialisedOnce; + + if (masterConfig.gyro_cal_on_first_arm && !armingCalibrationWasInitialisedOnce) { + gyroSetCalibrationCycles(calculateCalibratingCycles()); + armingCalibrationWasInitialisedOnce = true; } if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated From 7c83d23ddf568619287070205527fe757aa593a1 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 31 Mar 2016 22:58:22 +0200 Subject: [PATCH 21/78] Activate Iterm reset in normal mode too --- src/main/flight/pid.c | 20 ++++++++------------ src/main/io/serial_cli.c | 4 ++-- 2 files changed, 10 insertions(+), 14 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2da94c25d..b371d4ed5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -205,12 +205,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate I component. errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * getdT() * pidProfile->I8[axis] / 10.0f, -250.0f, 250.0f); - if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) { - if (axis == YAW) { - if (ABS(gyroRate) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; - } else { - if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; - } + if (axis == YAW) { + if (ABS(gyroRate) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; + } else { + if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; } if (antiWindupProtection || motorLimitReached) { @@ -492,12 +490,10 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // I coefficient (I8) moved before integration to make limiting independent from PID settings errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); - if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) { - if (axis == YAW) { - if (ABS(gyroRate / 4) >= pidProfile->yawItermResetRate) errorGyroI[axis] = 0; - } else { - if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = 0; - } + if (axis == YAW) { + if (ABS(gyroRate / 4) >= pidProfile->yawItermResetRate) errorGyroI[axis] = 0; + } else { + if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = 0; } if (antiWindupProtection || motorLimitReached) { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9aba1bd3b..1b2b67c52 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -727,11 +727,11 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, - { "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, + { "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } }, { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, - { "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, + { "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, From 24b97aed01bb02120edaea6e7207bac8f05265db Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 31 Mar 2016 23:59:19 +0200 Subject: [PATCH 22/78] Iterm reset only on yaw for now --- src/main/flight/pid.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b371d4ed5..9a1e96efa 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -205,10 +205,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate I component. errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * getdT() * pidProfile->I8[axis] / 10.0f, -250.0f, 250.0f); + if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) { + if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; + } + if (axis == YAW) { if (ABS(gyroRate) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; - } else { - if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; } if (antiWindupProtection || motorLimitReached) { @@ -490,10 +492,12 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // I coefficient (I8) moved before integration to make limiting independent from PID settings errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); + if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) { + if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = 0; + } + if (axis == YAW) { if (ABS(gyroRate / 4) >= pidProfile->yawItermResetRate) errorGyroI[axis] = 0; - } else { - if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = 0; } if (antiWindupProtection || motorLimitReached) { From 85dc6b59d24a4d905cc105ea98728a38ac755de0 Mon Sep 17 00:00:00 2001 From: 4712 <4712@outlook.de> Date: Fri, 1 Apr 2016 01:58:18 +0200 Subject: [PATCH 23/78] 1wire-pass-through-vcp also changed uart 1wire-pass-through to avoid need of FC reboot after exit. --- Makefile | 1 + src/main/drivers/pwm_output.c | 13 +- src/main/drivers/pwm_output.h | 2 + src/main/drivers/serial_usb_vcp.c | 6 + src/main/drivers/serial_usb_vcp.h | 1 + src/main/io/serial_1wire.c | 55 +++-- src/main/io/serial_1wire.h | 1 + src/main/io/serial_1wire_vcp.c | 237 ++++++++++++++++++++++ src/main/io/serial_1wire_vcp.h | 42 ++++ src/main/io/serial_msp.c | 133 ++++++++---- src/main/target/CC3D/target.h | 6 + src/main/target/COLIBRI_RACE/target.h | 6 + src/main/target/IRCFUSIONF3/target.h | 8 + src/main/target/LUX_RACE/target.h | 6 + src/main/target/MOTOLAB/target.h | 7 +- src/main/target/NAZE/target.h | 6 + src/main/target/PORT103R/target.h | 12 ++ src/main/target/RMDO/target.h | 6 + src/main/target/SPARKY/target.h | 6 + src/main/target/SPRACINGF3/target.h | 6 + src/main/target/SPRACINGF3MINI/target.h | 6 + src/main/target/STM32F3DISCOVERY/target.h | 9 +- src/main/vcp/hw_config.c | 13 ++ src/main/vcp/hw_config.h | 1 + src/main/vcp/usb_prop.c | 12 ++ src/main/vcp/usb_prop.h | 1 + 26 files changed, 533 insertions(+), 69 deletions(-) create mode 100644 src/main/io/serial_1wire_vcp.c create mode 100644 src/main/io/serial_1wire_vcp.h diff --git a/Makefile b/Makefile index f0234cc2b..fab52754f 100644 --- a/Makefile +++ b/Makefile @@ -305,6 +305,7 @@ COMMON_SRC = build_config.c \ io/rc_curves.c \ io/serial.c \ io/serial_1wire.c \ + io/serial_1wire_vcp.c \ io/serial_cli.c \ io/serial_msp.c \ io/statusindicator.c \ diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index cce25e340..d521153bf 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -50,6 +50,7 @@ static pwmOutputPort_t *servos[MAX_PWM_SERVOS]; static uint8_t allocatedOutputPortCount = 0; +static bool pwmMotorsEnabled = true; static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value) { TIM_OCInitTypeDef TIM_OCInitStructure; @@ -163,7 +164,7 @@ static void pwmWriteMultiShot(uint8_t index, uint16_t value) void pwmWriteMotor(uint8_t index, uint16_t value) { - if (motors[index] && index < MAX_MOTORS) + if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) motors[index]->pwmWritePtr(index, value); } @@ -177,6 +178,16 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount) } } +void pwmDisableMotors(void) +{ + pwmMotorsEnabled = false; +} + +void pwmEnableMotors(void) +{ + pwmMotorsEnabled = true; +} + void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) { uint8_t index; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 19c5aa820..11036d527 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -24,3 +24,5 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount); void pwmWriteServo(uint8_t index, uint16_t value); bool isMotorBrushed(uint16_t motorPwmRate); +void pwmDisableMotors(void); +void pwmEnableMotors(void); diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index ed1e50de1..c3f632b1c 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -183,3 +183,9 @@ serialPort_t *usbVcpOpen(void) return (serialPort_t *)s; } +uint32_t usbVcpGetBaudRate(serialPort_t *instance) +{ + UNUSED(instance); + + return CDC_BaudRate(); +} diff --git a/src/main/drivers/serial_usb_vcp.h b/src/main/drivers/serial_usb_vcp.h index 068073c2f..7d1b88be1 100644 --- a/src/main/drivers/serial_usb_vcp.h +++ b/src/main/drivers/serial_usb_vcp.h @@ -30,3 +30,4 @@ typedef struct { } vcpPort_t; serialPort_t *usbVcpOpen(void); +uint32_t usbVcpGetBaudRate(serialPort_t *instance); diff --git a/src/main/io/serial_1wire.c b/src/main/io/serial_1wire.c index 98eaa5b88..6b0a5e89a 100644 --- a/src/main/io/serial_1wire.c +++ b/src/main/io/serial_1wire.c @@ -32,6 +32,7 @@ #include "io/serial_1wire.h" #include "io/beeper.h" #include "drivers/pwm_mapping.h" +#include "drivers/pwm_output.h" #include "flight/mixer.h" uint8_t escCount; // we detect the hardware dynamically @@ -42,7 +43,7 @@ static void gpio_set_mode(GPIO_TypeDef* gpio, uint16_t pin, GPIO_Mode mode) { gpio_config_t cfg; cfg.pin = pin; cfg.mode = mode; - cfg.speed = Speed_10MHz; + cfg.speed = Speed_2MHz; gpioInit(gpio, &cfg); } @@ -51,8 +52,7 @@ static uint32_t GetPinPos(uint32_t pin) { for (pinPos = 0; pinPos < 16; pinPos++) { uint32_t pinMask = (0x1 << pinPos); if (pin & pinMask) { - // pos found - return pinPos; + return pinPos; } } return 0; @@ -61,11 +61,12 @@ static uint32_t GetPinPos(uint32_t pin) { void usb1WireInitialize() { escCount = 0; + pwmDisableMotors(); memset(&escHardware,0,sizeof(escHardware)); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmGetOutputConfiguration(); for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) { if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { - if(motor[pwmOutputConfiguration->portConfigurations[i].index] >0 ) { + if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) { escHardware[escCount].gpio = pwmOutputConfiguration->portConfigurations[i].timerHardware->gpio; escHardware[escCount].pin = pwmOutputConfiguration->portConfigurations[i].timerHardware->pin; escHardware[escCount].pinpos = GetPinPos(escHardware[escCount].pin); @@ -76,29 +77,37 @@ void usb1WireInitialize() } } +void usb1WireDeInitialize(void){ + for (uint8_t selected_esc = 0; selected_esc < (escCount); selected_esc++) { + gpio_set_mode(escHardware[selected_esc].gpio,escHardware[selected_esc].pin, Mode_AF_PP); //GPIO_Mode_IPU + } + escCount = 0; + pwmEnableMotors(); +} + #ifdef STM32F10X static volatile uint32_t in_cr_mask, out_cr_mask; static __IO uint32_t *cr; static void gpio_prep_vars(uint32_t escIndex) { - GPIO_TypeDef *gpio = escHardware[escIndex].gpio; - uint32_t pinpos = escHardware[escIndex].pinpos; - // mask out extra bits from pinmode, leaving just CNF+MODE - uint32_t inmode = Mode_IPU & 0x0F; - uint32_t outmode = (Mode_Out_PP & 0x0F) | Speed_10MHz; - // reference CRL or CRH, depending whether pin number is 0..7 or 8..15 - cr = &gpio->CRL + (pinpos / 8); - // offset to CNF and MODE portions of CRx register - uint32_t shift = (pinpos % 8) * 4; - // Read out current CRx value - in_cr_mask = out_cr_mask = *cr; - // Mask out 4 bits - in_cr_mask &= ~(0xF << shift); - out_cr_mask &= ~(0xF << shift); - // save current pinmode - in_cr_mask |= inmode << shift; - out_cr_mask |= outmode << shift; + GPIO_TypeDef *gpio = escHardware[escIndex].gpio; + uint32_t pinpos = escHardware[escIndex].pinpos; + // mask out extra bits from pinmode, leaving just CNF+MODE + uint32_t inmode = Mode_IPU & 0x0F; + uint32_t outmode = (Mode_Out_PP & 0x0F) | Speed_10MHz; + // reference CRL or CRH, depending whether pin number is 0..7 or 8..15 + cr = &gpio->CRL + (pinpos / 8); + // offset to CNF and MODE portions of CRx register + uint32_t shift = (pinpos % 8) * 4; + // Read out current CRx value + in_cr_mask = out_cr_mask = *cr; + // Mask out 4 bits + in_cr_mask &= ~(0xF << shift); + out_cr_mask &= ~(0xF << shift); + // save current pinmode + in_cr_mask |= inmode << shift; + out_cr_mask |= outmode << shift; } static void gpioSetOne(uint32_t escIndex, GPIO_Mode mode) { @@ -153,7 +162,7 @@ void usb1WirePassthrough(uint8_t escIndex) GPIO_ResetBits(S1W_RX_GPIO, S1W_RX_PIN); GPIO_ResetBits(S1W_TX_GPIO, S1W_TX_PIN); // configure gpio - gpio_set_mode(S1W_RX_GPIO, S1W_RX_PIN, Mode_IPU); + //gpio_set_mode(S1W_RX_GPIO, S1W_RX_PIN, Mode_IPU); gpio_set_mode(S1W_TX_GPIO, S1W_TX_PIN, Mode_Out_PP); #ifdef STM32F10X @@ -209,6 +218,8 @@ void usb1WirePassthrough(uint8_t escIndex) // we get here in case ct reached zero TX_SET_HIGH; RX_LED_OFF; + // reactivate serial port + gpio_set_mode(S1W_TX_GPIO, S1W_TX_PIN, Mode_AF_PP); // Enable all irq (for Hardware UART) __enable_irq(); return; diff --git a/src/main/io/serial_1wire.h b/src/main/io/serial_1wire.h index 1e560ed93..594cdd43a 100644 --- a/src/main/io/serial_1wire.h +++ b/src/main/io/serial_1wire.h @@ -33,4 +33,5 @@ typedef struct { void usb1WireInitialize(); void usb1WirePassthrough(uint8_t escIndex); +void usb1WireDeInitialize(void); #endif diff --git a/src/main/io/serial_1wire_vcp.c b/src/main/io/serial_1wire_vcp.c new file mode 100644 index 000000000..db93b4695 --- /dev/null +++ b/src/main/io/serial_1wire_vcp.c @@ -0,0 +1,237 @@ +/* + * 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 . + * + * Author 4712 + */ + +#include +#include +#include +#include +#include "platform.h" + +#ifdef USE_SERIAL_1WIRE_VCP +#include "drivers/gpio.h" +#include "drivers/light_led.h" +#include "drivers/system.h" +#include "io/serial_1wire_vcp.h" +#include "io/beeper.h" +#include "drivers/pwm_mapping.h" +#include "drivers/pwm_output.h" +#include "flight/mixer.h" +#include "io/serial_msp.h" +#include "drivers/buf_writer.h" +#include "drivers/serial_usb_vcp.h" + +uint8_t escCount; + +static escHardware_t escHardware[MAX_PWM_MOTORS]; + +static uint32_t GetPinPos(uint32_t pin) { + uint32_t pinPos; + for (pinPos = 0; pinPos < 16; pinPos++) { + uint32_t pinMask = (0x1 << pinPos); + if (pin & pinMask) { + return pinPos; + } + } + return 0; +} +static uint8_t selected_esc; + +#define ESC_IS_HI digitalIn(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) != Bit_RESET +#define ESC_IS_LO digitalIn(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) == Bit_RESET +#define ESC_SET_HI digitalHi(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) +#define ESC_SET_LO digitalLo(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) + +#define ESC_INPUT gpioInit(escHardware[selected_esc].gpio, &escHardware[selected_esc].gpio_config_INPUT) +#define ESC_OUTPUT gpioInit(escHardware[selected_esc].gpio, &escHardware[selected_esc].gpio_config_OUTPUT) + +void usb1WireInitializeVcp(void) +{ + pwmDisableMotors(); + selected_esc = 0; + memset(&escHardware, 0, sizeof(escHardware)); + pwmOutputConfiguration_t *pwmOutputConfiguration = pwmGetOutputConfiguration(); + for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) { + if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { + if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) { + escHardware[selected_esc].gpio = pwmOutputConfiguration->portConfigurations[i].timerHardware->gpio; + escHardware[selected_esc].pin = pwmOutputConfiguration->portConfigurations[i].timerHardware->pin; + escHardware[selected_esc].pinpos = GetPinPos(escHardware[selected_esc].pin); + escHardware[selected_esc].gpio_config_INPUT.pin = escHardware[selected_esc].pin; + escHardware[selected_esc].gpio_config_INPUT.speed = Speed_2MHz; // see pwmOutConfig() + escHardware[selected_esc].gpio_config_INPUT.mode = Mode_IPU; + escHardware[selected_esc].gpio_config_OUTPUT = escHardware[selected_esc].gpio_config_INPUT; + escHardware[selected_esc].gpio_config_OUTPUT.mode = Mode_Out_PP; + ESC_INPUT; + ESC_SET_HI; + selected_esc++; + } + } + } + escCount = selected_esc; + selected_esc = 0; +} + +void usb1WireDeInitializeVcp(void){ + for (selected_esc = 0; selected_esc < (escCount); selected_esc++) { + escHardware[selected_esc].gpio_config_OUTPUT.mode = Mode_AF_PP; // see pwmOutConfig() + ESC_OUTPUT; + ESC_SET_LO; + } + escCount = 0; + pwmEnableMotors(); +} + +#define START_BIT_TIMEOUT_MS 2 + +#define BIT_TIME (52) //52uS +#define BIT_TIME_HALVE (BIT_TIME >> 1) //26uS +#define START_BIT_TIME (BIT_TIME_HALVE + 1) +#define STOP_BIT_TIME ((BIT_TIME * 9) + BIT_TIME_HALVE) + +static void suart_putc_(uint8_t tx_b) +{ + uint32_t btime; + ESC_SET_LO; // 0 = start bit + btime = BIT_TIME + micros(); + while (micros() < btime); + for(uint8_t bit = 0; bit <8; bit++) + { + if(tx_b & 1) + { + ESC_SET_HI; // 1 + } + else + { + ESC_SET_LO; // 0 + } + btime = btime + BIT_TIME; + tx_b = (tx_b >> 1); + while (micros() < btime); + } + ESC_SET_HI; // 1 = stop bit + btime = btime + BIT_TIME; + while (micros() < btime); +} + + +static uint8_t suart_getc_(uint8_t *bt) +{ + uint32_t btime; + uint32_t start_time; + uint32_t stop_time; + uint32_t wait_start; + + *bt = 0; + + wait_start = millis() + START_BIT_TIMEOUT_MS; + while (ESC_IS_HI) { + // check for start bit begin + if (millis() > wait_start) { + return 0; + } + } + // start bit + start_time = micros(); + btime = start_time + START_BIT_TIME; + stop_time = start_time + STOP_BIT_TIME; + + while (micros() < btime); + + if (ESC_IS_HI) return 0; // check start bit + for (uint8_t bit=0;bit<8;bit++) + { + btime = btime + BIT_TIME; + while (micros() < btime); + if (ESC_IS_HI) + { + *bt |= (1 << bit); // 1 else 0 + } + } + while (micros() < stop_time); + + if (ESC_IS_LO) return 0; // check stop bit + + return 1; // OK +} +#define USE_TXRX_LED + +#ifdef USE_TXRX_LED +#define RX_LED_OFF LED0_OFF +#define RX_LED_ON LED0_ON +#ifdef LED1 +#define TX_LED_OFF LED1_OFF +#define TX_LED_ON LED1_ON +#else +#define TX_LED_OFF LED0_OFF +#define TX_LED_ON LED0_ON +#endif +#else +#define RX_LED_OFF +#define RX_LED_ON +#define TX_LED_OFF +#define TX_LED_ON +#endif + +// This method translates 2 wires (a tx and rx line) to 1 wire, by letting the +// RX line control when data should be read or written from the single line +void usb1WirePassthroughVcp(mspPort_t *mspPort, bufWriter_t *bufwriter, uint8_t escIndex) +{ +#ifdef BEEPER + // fix for buzzer often starts beeping continuously when the ESCs are read + // switch beeper silent here + // TODO (4712) do we need beeperSilence()? + // beeperSilence(); +#endif + + selected_esc = escIndex; + // TODO (4712) check all possible baud rate ok? + // uint32_t baudrate = serialGetBaudRate(mspPort->port); + // serialSetBaudRate(mspPort->port, 19200); + + while(usbVcpGetBaudRate(mspPort->port) != 4800) { + // esc line is in Mode_IPU by default + static uint8_t bt; + + if (suart_getc_(&bt)) { + RX_LED_ON; + serialBeginWrite(mspPort->port); + bufWriterAppend(bufwriter, bt); + while (suart_getc_(&bt)){ + bufWriterAppend(bufwriter, bt); + } + serialEndWrite(mspPort->port); + bufWriterFlush(bufwriter); + RX_LED_OFF; + } + if (serialRxBytesWaiting(mspPort->port)) { + ESC_OUTPUT; + TX_LED_ON; + do { + bt = serialRead(mspPort->port); + suart_putc_(bt); + } while(serialRxBytesWaiting(mspPort->port)); + ESC_INPUT; + TX_LED_OFF; + } + } + // serialSetBaudRate(mspPort->port, baudrate); + return; +} +#endif + diff --git a/src/main/io/serial_1wire_vcp.h b/src/main/io/serial_1wire_vcp.h new file mode 100644 index 000000000..0f4a6a3dc --- /dev/null +++ b/src/main/io/serial_1wire_vcp.h @@ -0,0 +1,42 @@ +/* + * 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 . + * + * Author 4712 + */ +#pragma once + +#include "platform.h" +#ifdef USE_SERIAL_1WIRE_VCP +#include "drivers/serial.h" +#include "drivers/buf_writer.h" +#include "drivers/pwm_mapping.h" +#include "io/serial_msp.h" + +extern uint8_t escCount; + +typedef struct { + GPIO_TypeDef* gpio; + uint16_t pinpos; + uint16_t pin; + gpio_config_t gpio_config_INPUT; + gpio_config_t gpio_config_OUTPUT; +} escHardware_t; + +void usb1WireInitializeVcp(void); +void usb1WireDeInitializeVcp(void); +void usb1WirePassthroughVcp(mspPort_t *mspPort, bufWriter_t *bufwriter, uint8_t escIndex); +#endif + diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index bd8ee07a5..3bc83fd93 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -96,6 +96,9 @@ #ifdef USE_SERIAL_1WIRE #include "io/serial_1wire.h" #endif +#ifdef USE_SERIAL_1WIRE_VCP +#include "io/serial_1wire_vcp.h" +#endif #ifdef USE_ESCSERIAL #include "drivers/serial_escserial.h" #endif @@ -1751,61 +1754,105 @@ static bool processInCommand(void) // switch all motor lines HI usb1WireInitialize(); // reply the count of ESC found - headSerialReply(1); + headSerialReply(2); serialize8(escCount); - + serialize8(currentPort->port->identifier); // 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 return true; - } - else { + } else if (i < escCount) { // Check for channel number 0..ESC_COUNT-1 - if (i < escCount) { - // because we do not come back after calling usb1WirePassthrough - // proceed with a success reply first - headSerialReply(0); - tailSerialReply(); - // flush the transmit buffer - bufWriterFlush(writer); - // wait for all data to send - waitForSerialPortToFinishTransmitting(currentPort->port); - // Start to activate here - // motor 1 => index 0 + // because we do not come back after calling usb1WirePassthrough + // proceed with a success reply first + headSerialReply(0); + tailSerialReply(); + // flush the transmit buffer + bufWriterFlush(writer); + // wait for all data to send + waitForSerialPortToFinishTransmitting(currentPort->port); + // Start to activate here + // motor 1 => index 0 - // search currentPort portIndex - /* next lines seems to be unnecessary, because the currentPort always point to the same mspPorts[portIndex] - uint8_t portIndex; - for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { - if (currentPort == &mspPorts[portIndex]) { - break; - } - } - */ - mspReleasePortIfAllocated(mspSerialPort); // CloseSerialPort also marks currentPort as UNUSED_PORT - usb1WirePassthrough(i); - // Wait a bit more to let App read the 0 byte and switch baudrate - // 2ms will most likely do the job, but give some grace time - delay(10); - // rebuild/refill currentPort structure, does openSerialPort if marked UNUSED_PORT - used ports are skiped - mspAllocateSerialPorts(&masterConfig.serialConfig); - /* restore currentPort and mspSerialPort - setCurrentPort(&mspPorts[portIndex]); // not needed same index will be restored - */ - // former used MSP uart is active again - // restore MSP_SET_1WIRE as current command for correct headSerialReply(0) - currentPort->cmdMSP = MSP_SET_1WIRE; - } else { - // ESC channel higher than max. allowed - // rem: BLHeliSuite will not support more than 8 - headSerialError(0); + // search currentPort portIndex + /* next lines seems to be unnecessary, because the currentPort always point to the same mspPorts[portIndex] + uint8_t portIndex; + for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + if (currentPort == &mspPorts[portIndex]) { + break; + } } + */ + // *mspReleasePortIfAllocated(mspSerialPort); // CloseSerialPort also marks currentPort as UNUSED_PORT + usb1WirePassthrough(i); + // Wait a bit more to let App read the 0 byte and/or switch baudrate + // 2ms will most likely do the job, but give some grace time + delay(10); + // rebuild/refill currentPort structure, does openSerialPort if marked UNUSED_PORT - used ports are skiped + // *mspAllocateSerialPorts(&masterConfig.serialConfig); + /* restore currentPort and mspSerialPort + setCurrentPort(&mspPorts[portIndex]); // not needed same index will be restored + */ + // former used MSP uart is active again + // restore MSP_SET_1WIRE as current command for correct headSerialReply(0) + //currentPort->cmdMSP = MSP_SET_1WIRE; + } else if (i == 0xFE) { + usb1WireDeInitialize(); + } else { + // ESC channel higher than max. allowed + 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 + +#ifdef USE_SERIAL_1WIRE_VCP + case MSP_SET_1WIRE: + // 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 + usb1WireInitializeVcp(); + // reply the count of ESC found + headSerialReply(2); + serialize8(escCount); + serialize8(currentPort->port->identifier); + // 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 + + return true; + } else if (i < escCount) { + // Check for channel number 0..ESC_COUNT-1 + // because we do not come back after calling usb1WirePassthrough + // proceed with a success reply first + headSerialReply(0); + tailSerialReply(); + // flush the transmit buffer + bufWriterFlush(writer); + // wait for all data to send + waitForSerialPortToFinishTransmitting(currentPort->port); + // Start to activate here + // motor 1 => index 0 + usb1WirePassthroughVcp(currentPort, writer, i); + // Wait a bit more to let App switch baudrate + delay(10); + // former used MSP uart is still active // proceed as usual with MSP commands // and wait to switch to next channel - // rem: App needs to call MSP_BOOT to deinitialize Passthrough + // rem: App needs to call MSP_SET_1WIRE(0xFE) to deinitialize Passthrough + } else if (i == 0xFE) { + usb1WireDeInitializeVcp(); + } else { + // ESC channel higher than max. allowed + headSerialError(0); } - break; + break; #endif #ifdef USE_ESCSERIAL diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index e57a64270..cc6dc885c 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -126,8 +126,13 @@ #define USE_SERVOS #define USE_CLI +#ifdef USE_VCP +#define USE_SERIAL_1WIRE_VCP +#else #define USE_SERIAL_1WIRE +#endif +#ifdef USE_SERIAL_1WIRE // FlexPort (pin 21/22, TX/RX respectively): // Note, FlexPort has 10k pullups on both TX and RX // JST Pin3 TX - connect to external UART/USB RX @@ -136,6 +141,7 @@ // JST Pin4 RX - connect to external UART/USB TX #define S1W_RX_GPIO GPIOB #define S1W_RX_PIN GPIO_Pin_11 +#endif #undef DISPLAY #undef SONAR diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index d267b553c..add19d9d7 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -184,8 +184,14 @@ #define USE_SERVOS #define USE_CLI +#ifdef USE_VCP +#define USE_SERIAL_1WIRE_VCP +#else #define USE_SERIAL_1WIRE +#endif +#ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO GPIOB #define S1W_TX_PIN GPIO_Pin_10 #define S1W_RX_GPIO GPIOB #define S1W_RX_PIN GPIO_Pin_11 +#endif diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 4152bbfb0..d7ca4e76b 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -165,8 +165,16 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_11 +#ifdef USE_VCP +#define USE_SERIAL_1WIRE_VCP +#else #define USE_SERIAL_1WIRE +#endif +#ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO GPIOA #define S1W_TX_PIN GPIO_Pin_9 #define S1W_RX_GPIO GPIOA #define S1W_RX_PIN GPIO_Pin_10 +#endif + + diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index df441c319..f8c87d3e5 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -165,9 +165,15 @@ #define BIND_PORT GPIOC #define BIND_PIN Pin_5 +#ifdef USE_VCP +#define USE_SERIAL_1WIRE_VCP +#else #define USE_SERIAL_1WIRE +#endif +#ifdef USE_SERIAL_1WIRE // Untested #define S1W_TX_GPIO GPIOB #define S1W_TX_PIN GPIO_Pin_10 #define S1W_RX_GPIO GPIOB #define S1W_RX_PIN GPIO_Pin_11 +#endif diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index f49661f85..a123d5325 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -191,10 +191,15 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_4 +#ifdef USE_VCP +#define USE_SERIAL_1WIRE_VCP +#else #define USE_SERIAL_1WIRE +#endif +#ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO GPIOB #define S1W_TX_PIN GPIO_Pin_6 #define S1W_RX_GPIO GPIOB #define S1W_RX_PIN GPIO_Pin_7 - +#endif diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 7ec9848ae..de468e0b6 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -191,14 +191,20 @@ #define BIND_PORT GPIOA #define BIND_PIN Pin_3 +#ifdef USE_VCP +#define USE_SERIAL_1WIRE_VCP +#else #define USE_SERIAL_1WIRE +#endif +#ifdef USE_SERIAL_1WIRE // STM32F103CBT6-LQFP48 Pin30 (PA9) TX - PC3 connects to onboard CP2102 RX #define S1W_TX_GPIO GPIOA #define S1W_TX_PIN GPIO_Pin_9 // STM32F103CBT6-LQFP48 Pin31 (PA10) RX - PC1 to onboard CP2102 TX #define S1W_RX_GPIO GPIOA #define S1W_RX_PIN GPIO_Pin_10 +#endif // alternative defaults for AlienWii32 F1 target #ifdef ALIENWII32 diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 07ce1e2b2..64436374f 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -156,3 +156,15 @@ #define TELEMETRY #define USE_SERVOS #define USE_CLI +#ifdef USE_VCP +#define USE_SERIAL_1WIRE_VCP +#else +#define USE_SERIAL_1WIRE +#endif + +#ifdef USE_SERIAL_1WIRE +#define S1W_TX_GPIO GPIOA +#define S1W_TX_PIN GPIO_Pin_9 +#define S1W_RX_GPIO GPIOA +#define S1W_RX_PIN GPIO_Pin_10 +#endif diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 64e5f1246..f8b72d1d8 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -157,9 +157,15 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_11 +#ifdef USE_VCP +#define USE_SERIAL_1WIRE_VCP +#else #define USE_SERIAL_1WIRE +#endif +#ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO GPIOA #define S1W_TX_PIN GPIO_Pin_9 #define S1W_RX_GPIO GPIOA #define S1W_RX_PIN GPIO_Pin_10 +#endif diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 907a3cb09..d727f8c5b 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -166,12 +166,18 @@ #define WS2811_IRQ DMA1_Channel7_IRQn #endif +#ifdef USE_VCP +#define USE_SERIAL_1WIRE_VCP +#else #define USE_SERIAL_1WIRE +#endif +#ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO GPIOB #define S1W_TX_PIN GPIO_Pin_6 #define S1W_RX_GPIO GPIOB #define S1W_RX_PIN GPIO_Pin_7 +#endif #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index bd4c60bba..08937aac7 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -165,9 +165,15 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_11 +#ifdef USE_VCP +#define USE_SERIAL_1WIRE_VCP +#else #define USE_SERIAL_1WIRE +#endif +#ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO GPIOA #define S1W_TX_PIN GPIO_Pin_9 #define S1W_RX_GPIO GPIOA #define S1W_RX_PIN GPIO_Pin_10 +#endif diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 0fd8b2232..08d6bf7c9 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -234,9 +234,15 @@ #define BINDPLUG_PORT BUTTON_B_PORT #define BINDPLUG_PIN BUTTON_B_PIN +#ifdef USE_VCP +#define USE_SERIAL_1WIRE_VCP +#else #define USE_SERIAL_1WIRE +#endif +#ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO UART1_GPIO #define S1W_TX_PIN UART1_TX_PIN #define S1W_RX_GPIO UART1_GPIO #define S1W_RX_PIN UART1_RX_PIN +#endif diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 5796239f4..0b8e1bb84 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -162,12 +162,17 @@ #define USE_SERVOS #define USE_CLI +#ifdef USE_VCP +#define USE_SERIAL_1WIRE_VCP +#else #define USE_SERIAL_1WIRE -// How many escs does this board support? -#define ESC_COUNT 6 +#endif + +#ifdef USE_SERIAL_1WIRE // 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 +#endif diff --git a/src/main/vcp/hw_config.c b/src/main/vcp/hw_config.c index bad9674a1..7384b1e0b 100644 --- a/src/main/vcp/hw_config.c +++ b/src/main/vcp/hw_config.c @@ -362,4 +362,17 @@ uint8_t usbIsConnected(void) return (bDeviceState != UNCONNECTED); } + +/******************************************************************************* + * Function Name : CDC_BaudRate. + * Description : Get the current baud rate + * Input : None. + * Output : None. + * Return : Baud rate in bps + *******************************************************************************/ +uint32_t CDC_BaudRate(void) +{ + return Virtual_Com_Port_GetBaudRate(); +} + /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/vcp/hw_config.h b/src/main/vcp/hw_config.h index ee40ed708..de454859f 100644 --- a/src/main/vcp/hw_config.h +++ b/src/main/vcp/hw_config.h @@ -59,6 +59,7 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength); // HJI uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len); // HJI uint8_t usbIsConfigured(void); // HJI uint8_t usbIsConnected(void); // HJI +uint32_t CDC_BaudRate(void); /* External variables --------------------------------------------------------*/ extern __IO uint32_t receiveLength; // HJI diff --git a/src/main/vcp/usb_prop.c b/src/main/vcp/usb_prop.c index 0130ecd71..b070ce9cc 100644 --- a/src/main/vcp/usb_prop.c +++ b/src/main/vcp/usb_prop.c @@ -358,5 +358,17 @@ uint8_t *Virtual_Com_Port_SetLineCoding(uint16_t Length) return (uint8_t *)&linecoding; } +/******************************************************************************* + * Function Name : Virtual_Com_Port_GetBaudRate. + * Description : Get the current baudrate + * Input : None. + * Output : None. + * Return : baudrate in bps + *******************************************************************************/ +uint32_t Virtual_Com_Port_GetBaudRate(void) +{ + return linecoding.bitrate; +} + /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/vcp/usb_prop.h b/src/main/vcp/usb_prop.h index b9b379c45..12d050c38 100644 --- a/src/main/vcp/usb_prop.h +++ b/src/main/vcp/usb_prop.h @@ -79,6 +79,7 @@ uint8_t *Virtual_Com_Port_GetStringDescriptor( uint16_t); uint8_t *Virtual_Com_Port_GetLineCoding(uint16_t Length); uint8_t *Virtual_Com_Port_SetLineCoding(uint16_t Length); +uint32_t Virtual_Com_Port_GetBaudRate(void); #endif /* __usb_prop_H */ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ From 68587452cad09b5724fce37ebb2ab8aa7fb28ded Mon Sep 17 00:00:00 2001 From: Brian Balogh Date: Fri, 1 Apr 2016 02:44:48 -0400 Subject: [PATCH 24/78] Point yaw_lowpass_hz CLI var at yaw_lpf_hz --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 1b2b67c52..572f4ffcb 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -731,7 +731,7 @@ const clivalue_t valueTable[] = { { "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } }, { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, - { "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, + { "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, From d32205d8565b7274eae5da50b05e743bead06b69 Mon Sep 17 00:00:00 2001 From: 4712 <4712@outlook.de> Date: Sat, 2 Apr 2016 17:07:12 +0200 Subject: [PATCH 25/78] 4way-interface --- Makefile | 3 + src/main/io/serial_4way.c | 775 ++++++++++++++++++++++ src/main/io/serial_4way.h | 95 +++ src/main/io/serial_4way_avrootloader.c | 363 ++++++++++ src/main/io/serial_4way_avrootloader.h | 34 + src/main/io/serial_4way_stk500v2.c | 430 ++++++++++++ src/main/io/serial_4way_stk500v2.h | 31 + src/main/io/serial_msp.c | 26 + src/main/io/serial_msp.h | 1 + src/main/target/CC3D/target.h | 5 + src/main/target/COLIBRI_RACE/target.h | 6 + src/main/target/IRCFUSIONF3/target.h | 6 + src/main/target/LUX_RACE/target.h | 6 + src/main/target/MOTOLAB/target.h | 5 + src/main/target/NAZE/target.h | 5 + src/main/target/PORT103R/target.h | 6 + src/main/target/RMDO/target.h | 5 + src/main/target/SPARKY/target.h | 5 + src/main/target/SPRACINGF3/target.h | 5 + src/main/target/SPRACINGF3MINI/target.h | 5 + src/main/target/STM32F3DISCOVERY/target.h | 5 + 21 files changed, 1822 insertions(+) create mode 100644 src/main/io/serial_4way.c create mode 100644 src/main/io/serial_4way.h create mode 100644 src/main/io/serial_4way_avrootloader.c create mode 100644 src/main/io/serial_4way_avrootloader.h create mode 100644 src/main/io/serial_4way_stk500v2.c create mode 100644 src/main/io/serial_4way_stk500v2.h diff --git a/Makefile b/Makefile index fab52754f..b5ff16703 100644 --- a/Makefile +++ b/Makefile @@ -306,6 +306,9 @@ COMMON_SRC = build_config.c \ io/serial.c \ io/serial_1wire.c \ io/serial_1wire_vcp.c \ + io/serial_4way.c \ + io/serial_4way_avrootloader.c \ + io/serial_4way_stk500v2.c \ io/serial_cli.c \ io/serial_msp.c \ io/statusindicator.c \ diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c new file mode 100644 index 000000000..4be6c1364 --- /dev/null +++ b/src/main/io/serial_4way.c @@ -0,0 +1,775 @@ +/* + * 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 . + * Author: 4712 +*/ +#include "platform.h" +#if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) +#include +#include +#include +#include +#include +#include "drivers/system.h" +#include "drivers/pwm_output.h" +#include "flight/mixer.h" +#include "io/beeper.h" +#include "io/serial_4way.h" +#include "io/serial_msp.h" +#include "drivers/buf_writer.h" + +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#include "io/serial_4way_avrootloader.h" +#endif +#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) +#include "io/serial_4way_stk500v2.h" +#endif + +#define SERIAL_4WAY_INTERFACE_NAME_STR "m4wFCIntf" +// *** change to adapt Revision +#define SERIAL_4WAY_VER_MAIN 14 +#define SERIAL_4WAY_VER_SUB_1 (uint8_t) 4 +#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 03 + +#define SERIAL_4WAY_PROTOCOL_VER 106 +// *** end + +#if (SERIAL_4WAY_VER_MAIN > 24) +#error "beware of SERIAL_4WAY_VER_SUB_1 is uint8_t" +#endif + + +#define SERIAL_4WAY_VERSION (uint16_t) ((SERIAL_4WAY_VER_MAIN * 1000) + (SERIAL_4WAY_VER_SUB_1 * 100) + SERIAL_4WAY_VER_SUB_2) + +#define SERIAL_4WAY_VERSION_HI (uint8_t) (SERIAL_4WAY_VERSION / 100) +#define SERIAL_4WAY_VERSION_LO (uint8_t) (SERIAL_4WAY_VERSION % 100) + +static uint8_t escCount; + +static uint32_t GetPinPos(uint32_t pin) +{ + uint32_t pinPos; + for (pinPos = 0; pinPos < 16; pinPos++) { + uint32_t pinMask = (0x1 << pinPos); + if (pin & pinMask) { + return pinPos; + } + } + return 0; +} + +uint8_t Initialize4WayInterface(void) +{ + // StopPwmAllMotors(); + pwmDisableMotors(); + selected_esc = 0; + memset(&escHardware, 0, sizeof(escHardware)); + pwmOutputConfiguration_t *pwmOutputConfiguration = pwmGetOutputConfiguration(); + for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) { + if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { + if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) { + escHardware[selected_esc].gpio = pwmOutputConfiguration->portConfigurations[i].timerHardware->gpio; + escHardware[selected_esc].pin = pwmOutputConfiguration->portConfigurations[i].timerHardware->pin; + escHardware[selected_esc].pinpos = GetPinPos(escHardware[selected_esc].pin); + escHardware[selected_esc].gpio_config_INPUT.pin = escHardware[selected_esc].pin; + escHardware[selected_esc].gpio_config_INPUT.speed = Speed_2MHz; // see pwmOutConfig() + escHardware[selected_esc].gpio_config_INPUT.mode = Mode_IPU; + escHardware[selected_esc].gpio_config_OUTPUT = escHardware[selected_esc].gpio_config_INPUT; + escHardware[selected_esc].gpio_config_OUTPUT.mode = Mode_Out_PP; + ESC_INPUT; + ESC_SET_HI; + selected_esc++; + } + } + } + escCount = selected_esc; + return escCount; +} + +void DeInitialize4WayInterface(void) +{ + for (selected_esc = 0; selected_esc < (escCount); selected_esc++) { + escHardware[selected_esc].gpio_config_OUTPUT.mode = Mode_AF_PP; // see pwmOutConfig() + ESC_OUTPUT; + ESC_SET_LO; + } + escCount = 0; + pwmEnableMotors(); +} + + +#define SET_DISCONNECTED DeviceInfo.words[0] = 0 + +#define INTF_MODE_IDX 3 // index for DeviceInfostate + +// Interface related only +// establish and test connection to the Interface + +// Send Structure +// ESC + CMD PARAM_LEN [PARAM (if len > 0)] CRC16_Hi CRC16_Lo +// Return +// ESC CMD PARAM_LEN [PARAM (if len > 0)] + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo + +#define cmd_Remote_Escape 0x2E // '.' +#define cmd_Local_Escape 0x2F // '/' + +// Test Interface still present +#define cmd_InterfaceTestAlive 0x30 // '0' alive +// RETURN: ACK + +// get Protocol Version Number 01..255 +#define cmd_ProtocolGetVersion 0x31 // '1' version +// RETURN: uint8_t VersionNumber + ACK + +// get Version String +#define cmd_InterfaceGetName 0x32 // '2' name +// RETURN: String + ACK + +//get Version Number 01..255 +#define cmd_InterfaceGetVersion 0x33 // '3' version +// RETURN: uint8_t AVersionNumber + ACK + + +// Exit / Restart Interface - can be used to switch to Box Mode +#define cmd_InterfaceExit 0x34 // '4' exit +// RETURN: ACK + +// Reset the Device connected to the Interface +#define cmd_DeviceReset 0x35 // '5' reset +// RETURN: ACK + +// Get the Device ID connected +// #define cmd_DeviceGetID 0x36 //'6' device id removed since 06/106 +// RETURN: uint8_t DeviceID + ACK + +// Initialize Flash Access for Device connected +#define cmd_DeviceInitFlash 0x37 // '7' init flash access +// RETURN: ACK + +// Erase the whole Device Memory of connected Device +#define cmd_DeviceEraseAll 0x38 // '8' erase all +// RETURN: ACK + +// Erase one Page of Device Memory of connected Device +#define cmd_DevicePageErase 0x39 // '9' page erase +// PARAM: uint8_t APageNumber +// RETURN: ACK + +// Read to Buffer from Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceRead 0x3A // ':' read Device +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255] +// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK + +// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceWrite 0x3B // ';' write +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] +// RETURN: ACK + +// Set C2CK low infinite ) permanent Reset state +#define cmd_DeviceC2CK_LOW 0x3C // '<' +// RETURN: ACK + +// Read to Buffer from Device Memory of connected Device //Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceReadEEprom 0x3D // '=' read Device +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255] +// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK + +// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceWriteEEprom 0x3E // '>' write +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] +// RETURN: ACK + +// Set Interface Mode +#define cmd_InterfaceSetMode 0x3F // '?' +// #define imC2 0 +// #define imSIL_BLB 1 +// #define imATM_BLB 2 +// #define imSK 3 +// PARAM: uint8_t Mode +// RETURN: ACK or ACK_I_INVALID_CHANNEL + +// responses +#define ACK_OK 0x00 +// #define ACK_I_UNKNOWN_ERROR 0x01 +#define ACK_I_INVALID_CMD 0x02 +#define ACK_I_INVALID_CRC 0x03 +#define ACK_I_VERIFY_ERROR 0x04 +// #define ACK_D_INVALID_COMMAND 0x05 +// #define ACK_D_COMMAND_FAILED 0x06 +// #define ACK_D_UNKNOWN_ERROR 0x07 + +#define ACK_I_INVALID_CHANNEL 0x08 +#define ACK_I_INVALID_PARAM 0x09 +#define ACK_D_GENERAL_ERROR 0x0F + +/* Copyright (c) 2002, 2003, 2004 Marek Michalkiewicz + Copyright (c) 2005, 2007 Joerg Wunsch + Copyright (c) 2013 Dave Hylands + Copyright (c) 2013 Frederic Nadeau + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + + * Neither the name of the copyright holders nor the names of + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. */ +uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) { + int i; + + crc = crc ^ ((uint16_t)data << 8); + for (i=0; i < 8; i++){ + if (crc & 0x8000) + crc = (crc << 1) ^ 0x1021; + else + crc <<= 1; + } + return crc; +} +// * End copyright + + +#define ATMEL_DEVICE_MATCH ((DeviceInfo.words[0] == 0x9307) || (DeviceInfo.words[0] == 0x930A) || \ + (DeviceInfo.words[0] == 0x930F) || (DeviceInfo.words[0] == 0x940B)) + +#define SILABS_DEVICE_MATCH ((DeviceInfo.words[0] == 0xF310)||(DeviceInfo.words[0] ==0xF330) || \ + (DeviceInfo.words[0] == 0xF410) || (DeviceInfo.words[0] == 0xF390) || \ + (DeviceInfo.words[0] == 0xF850) || (DeviceInfo.words[0] == 0xE8B1) || \ + (DeviceInfo.words[0] == 0xE8B2)) + +static uint8_t CurrentInterfaceMode; + +static uint8_t Connect(void) +{ + for (uint8_t I = 0; I < 3; ++I) { + #if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) + if (Stk_ConnectEx() && ATMEL_DEVICE_MATCH) { + CurrentInterfaceMode = imSK; + return 1; + } else { + if (BL_ConnectEx()) { + if SILABS_DEVICE_MATCH { + CurrentInterfaceMode = imSIL_BLB; + return 1; + } else if ATMEL_DEVICE_MATCH { + CurrentInterfaceMode = imATM_BLB; + return 1; + } + } + } + #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) + if (BL_ConnectEx()) { + if SILABS_DEVICE_MATCH { + CurrentInterfaceMode = imSIL_BLB; + return 1; + } else if ATMEL_DEVICE_MATCH { + CurrentInterfaceMode = imATM_BLB; + return 1; + } + } + #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if (Stk_ConnectEx()) { + CurrentInterfaceMode = imSK; + if ATMEL_DEVICE_MATCH return 1; + } + #endif + } + return 0; +} + +static mspPort_t *_mspPort; +static bufWriter_t *_writer; + +static uint8_t ReadByte(void) { + // need timeout? + while (!serialRxBytesWaiting(_mspPort->port)); + return serialRead(_mspPort->port); +} + +static union uint8_16u CRC_in; +static uint8_t ReadByteCrc(void) { + uint8_t b = ReadByte(); + CRC_in.word = _crc_xmodem_update(CRC_in.word, b); + return b; +} +static void WriteByte(uint8_t b){ + bufWriterAppend(_writer, b); +} + +static union uint8_16u CRCout; +static void WriteByteCrc(uint8_t b){ + WriteByte(b); + CRCout.word = _crc_xmodem_update(CRCout.word, b); +} + +void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { + + uint8_t ParamBuf[256]; + uint8_t ESC; + uint8_t I_PARAM_LEN; + uint8_t CMD; + uint8_t ACK_OUT; + union uint8_16u CRC_check; + union uint8_16u Dummy; + uint8_t O_PARAM_LEN; + uint8_t *O_PARAM; + uint8_t *InBuff; + + _mspPort = mspPort; + _writer = bufwriter; + + // Start here with UART Main loop + #ifdef BEEPER + // fix for buzzer often starts beeping continuously when the ESCs are read + // switch beeper silent here + // TODO (4712) check: is beeperSilence useful here? + beeperSilence(); + #endif + bool isExitScheduled = false; + + while(1) { + // restart looking for new sequence from host + do { + CRC_in.word = 0; + ESC = ReadByteCrc(); + } while (ESC != cmd_Local_Escape); + + RX_LED_ON; + + Dummy.word = 0; + O_PARAM = &Dummy.bytes[0]; + O_PARAM_LEN = 1; + CMD = ReadByteCrc(); + D_FLASH_ADDR_H = ReadByteCrc(); + D_FLASH_ADDR_L = ReadByteCrc(); + I_PARAM_LEN = ReadByteCrc(); + + InBuff = ParamBuf; + uint8_t i = I_PARAM_LEN; + do { + *InBuff = ReadByteCrc(); + InBuff++; + i--; + } while (i != 0); + + CRC_check.bytes[1] = ReadByte(); + CRC_check.bytes[0] = ReadByte(); + + RX_LED_OFF; + + if(CRC_check.word == CRC_in.word) { + ACK_OUT = ACK_OK; + } else { + ACK_OUT = ACK_I_INVALID_CRC; + } + + if (ACK_OUT == ACK_OK) + { + // D_FLASH_ADDR_H=Adress_H; + // D_FLASH_ADDR_L=Adress_L; + D_PTR_I = ParamBuf; + + switch(CMD) { + // ******* Interface related stuff ******* + case cmd_InterfaceTestAlive: + { + break; + if (IsMcuConnected){ + switch(CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + case imSIL_BLB: + { + if (!BL_SendCMDKeepAlive()) { // SetStateDisconnected() included + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_SignOn()) { // SetStateDisconnected(); + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_D_GENERAL_ERROR; + } + if ( ACK_OUT != ACK_OK) SET_DISCONNECTED; + } + break; + } + case cmd_ProtocolGetVersion: + { + // Only interface itself, no matter what Device + Dummy.bytes[0] = SERIAL_4WAY_PROTOCOL_VER; + break; + } + + case cmd_InterfaceGetName: + { + // Only interface itself, no matter what Device + // O_PARAM_LEN=16; + O_PARAM_LEN = strlen(SERIAL_4WAY_INTERFACE_NAME_STR); + O_PARAM = (uint8_t *)SERIAL_4WAY_INTERFACE_NAME_STR; + break; + } + + case cmd_InterfaceGetVersion: + { + // Only interface itself, no matter what Device + // Dummy = iUart_res_InterfVersion; + O_PARAM_LEN = 2; + Dummy.bytes[0] = SERIAL_4WAY_VERSION_HI; + Dummy.bytes[1] = SERIAL_4WAY_VERSION_LO; + break; + } + case cmd_InterfaceExit: + { + isExitScheduled = true; + break; + } + case cmd_InterfaceSetMode: + { + #if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if ((ParamBuf[0] <= imSK) && (ParamBuf[0] >= imSIL_BLB)) { + #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) + if ((ParamBuf[0] <= imATM_BLB) && (ParamBuf[0] >= imSIL_BLB)) { + #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if (ParamBuf[0] == imSK) { + #endif + CurrentInterfaceMode = ParamBuf[0]; + } else { + ACK_OUT = ACK_I_INVALID_PARAM; + } + break; + } + + case cmd_DeviceReset: + { + if (ParamBuf[0] < escCount) { + // Channel may change here + selected_esc = ParamBuf[0]; + } + else { + ACK_OUT = ACK_I_INVALID_CHANNEL; + break; + } + switch (CurrentInterfaceMode) + { + case imSIL_BLB: + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + { + BL_SendCMDRunRestartBootloader(); + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + break; + } + #endif + } + SET_DISCONNECTED; + break; + } + case cmd_DeviceInitFlash: + { + SET_DISCONNECTED; + if (ParamBuf[0] < escCount) { + //Channel may change here + //ESC_LO or ESC_HI; Halt state for prev channel + selected_esc = ParamBuf[0]; + } else { + ACK_OUT = ACK_I_INVALID_CHANNEL; + break; + } + O_PARAM_LEN = DeviceInfoSize; //4 + O_PARAM = (uint8_t *)&DeviceInfo; + if(Connect()) { + DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode; + } else { + SET_DISCONNECTED; + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case cmd_DeviceEraseAll: + { + switch(CurrentInterfaceMode) + { + case imSK: + { + if (!Stk_Chip_Erase()) ACK_OUT=ACK_D_GENERAL_ERROR; + break; + } + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + break; + } + #endif + + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case cmd_DevicePageErase: + { + switch (CurrentInterfaceMode) + { + case imSIL_BLB: + { + Dummy.bytes[0] = ParamBuf[0]; + //Address = Page * 512 + D_FLASH_ADDR_H = (Dummy.bytes[0] << 1); + D_FLASH_ADDR_L = 0; + if (!BL_PageErase()) ACK_OUT = ACK_D_GENERAL_ERROR; + break; + } + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + break; + } + #endif + + //*** Device Memory Read Ops *** + case cmd_DeviceRead: + { + D_NUM_BYTES = ParamBuf[0]; + /* + D_FLASH_ADDR_H=Adress_H; + D_FLASH_ADDR_L=Adress_L; + D_PTR_I = BUF_I; + */ + switch(CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + case imATM_BLB: + { + if(!BL_ReadFlash(CurrentInterfaceMode)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if(!Stk_ReadFlash()) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + if (ACK_OUT == ACK_OK) + { + O_PARAM_LEN = D_NUM_BYTES; + O_PARAM = (uint8_t *)&ParamBuf; + } + break; + } + + case cmd_DeviceReadEEprom: + { + D_NUM_BYTES = ParamBuf[0]; + /* + D_FLASH_ADDR_H = Adress_H; + D_FLASH_ADDR_L = Adress_L; + D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + { + if (!BL_ReadEEprom()) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_ReadEEprom()) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + if(ACK_OUT == ACK_OK) + { + O_PARAM_LEN = D_NUM_BYTES; + O_PARAM = (uint8_t *)&ParamBuf; + } + break; + } + + //*** Device Memory Write Ops *** + case cmd_DeviceWrite: + { + D_NUM_BYTES = I_PARAM_LEN; + /* + D_FLASH_ADDR_H=Adress_H; + D_FLASH_ADDR_L=Adress_L; + D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + case imATM_BLB: + { + if (!BL_WriteFlash()) { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_WriteFlash()) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + } + break; + } + + case cmd_DeviceWriteEEprom: + { + D_NUM_BYTES = I_PARAM_LEN; + ACK_OUT = ACK_D_GENERAL_ERROR; + /* + D_FLASH_ADDR_H=Adress_H; + D_FLASH_ADDR_L=Adress_L; + D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + { + ACK_OUT = ACK_I_INVALID_CMD; + break; + } + case imATM_BLB: + { + if (BL_WriteEEprom()) + { + ACK_OUT = ACK_OK; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (Stk_WriteEEprom()) + { + ACK_OUT = ACK_OK; + } + break; + } + #endif + } + break; + } + default: + { + ACK_OUT = ACK_I_INVALID_CMD; + } + } + } + + CRCout.word = 0; + + TX_LED_ON; + serialBeginWrite(_mspPort->port); + WriteByteCrc(cmd_Remote_Escape); + WriteByteCrc(CMD); + WriteByteCrc(D_FLASH_ADDR_H); + WriteByteCrc(D_FLASH_ADDR_L); + WriteByteCrc(O_PARAM_LEN); + + i=O_PARAM_LEN; + do { + WriteByteCrc(*O_PARAM); + O_PARAM++; + i--; + } while (i > 0); + + WriteByteCrc(ACK_OUT); + WriteByte(CRCout.bytes[1]); + WriteByte(CRCout.bytes[0]); + serialEndWrite(_mspPort->port); + bufWriterFlush(_writer); + TX_LED_OFF; + if (isExitScheduled) { + DeInitialize4WayInterface(); + return; + } + }; +} + + + +#endif diff --git a/src/main/io/serial_4way.h b/src/main/io/serial_4way.h new file mode 100644 index 000000000..853b120a2 --- /dev/null +++ b/src/main/io/serial_4way.h @@ -0,0 +1,95 @@ +/* + * 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 . + * Author: 4712 +*/ + +#include +#if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) +#include "drivers/serial.h" +#include "drivers/buf_writer.h" +#include "drivers/gpio.h" +#include "drivers/timer.h" +#include "drivers/pwm_mapping.h" +#include "drivers/light_led.h" +#include "io/serial_msp.h" + + +#define imC2 0 +#define imSIL_BLB 1 +#define imATM_BLB 2 +#define imSK 3 + +#define RX_LED_OFF LED0_OFF +#define RX_LED_ON LED0_ON +#define TX_LED_OFF LED1_OFF +#define TX_LED_ON LED1_ON + +typedef struct { + GPIO_TypeDef* gpio; + uint16_t pinpos; + uint16_t pin; + gpio_config_t gpio_config_INPUT; + gpio_config_t gpio_config_OUTPUT; +} escHardware_t; + +uint8_t selected_esc; + +escHardware_t escHardware[MAX_PWM_MOTORS]; + +#define ESC_IS_HI digitalIn(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) != Bit_RESET +#define ESC_IS_LO digitalIn(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) == Bit_RESET +#define ESC_SET_HI digitalHi(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) +#define ESC_SET_LO digitalLo(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) + +#define ESC_INPUT gpioInit(escHardware[selected_esc].gpio, &escHardware[selected_esc].gpio_config_INPUT) +#define ESC_OUTPUT gpioInit(escHardware[selected_esc].gpio, &escHardware[selected_esc].gpio_config_OUTPUT) + + + +void delay_us(uint32_t delay); + +union __attribute__ ((packed)) uint8_16u +{ + uint8_t bytes[2]; + uint16_t word; +}; + +union __attribute__ ((packed)) uint8_32u +{ + uint8_t bytes[4]; + uint16_t words[2]; + uint32_t dword; +}; + +//----------------------------------------------------------------------------------- +// Global VARIABLES +//----------------------------------------------------------------------------------- +uint8_t D_NUM_BYTES; +uint8_t D_FLASH_ADDR_H; +uint8_t D_FLASH_ADDR_L; +uint8_t *D_PTR_I; + +#define DeviceInfoSize 4 + +union uint8_32u DeviceInfo; + +#define IsMcuConnected (DeviceInfo.bytes[0] > 0) + +uint8_t Initialize4WayInterface(void); +void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter); +void DeInitialize4WayInterface(void); + +#endif diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c new file mode 100644 index 000000000..a2c2f6890 --- /dev/null +++ b/src/main/io/serial_4way_avrootloader.c @@ -0,0 +1,363 @@ +/* + * 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 . + * Author: 4712 + * for info about Hagens AVRootloader: + * http://www.mikrocontroller.net/topic/avr-bootloader-mit-verschluesselung +*/ + +#include +#include +#include +#include +#include + +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#include "drivers/system.h" +#include "io/serial_4way_avrootloader.h" +#include "io/serial_4way.h" + +// Bootloader commands +// RunCmd +#define RestartBootloader 0 +#define ExitBootloader 1 + +#define CMD_RUN 0x00 +#define CMD_PROG_FLASH 0x01 +#define CMD_ERASE_FLASH 0x02 +#define CMD_READ_FLASH_SIL 0x03 +#define CMD_VERIFY_FLASH 0x03 +#define CMD_READ_EEPROM 0x04 +#define CMD_PROG_EEPROM 0x05 +#define CMD_READ_SRAM 0x06 +#define CMD_READ_FLASH_ATM 0x07 +#define CMD_KEEP_ALIVE 0xFD +#define CMD_SET_ADDRESS 0xFF +#define CMD_SET_BUFFER 0xFE + +#define CMD_BOOTINIT 0x07 +#define CMD_BOOTSIGN 0x08 + +// Bootloader result codes + +#define brSUCCESS 0x30 +#define brERRORCOMMAND 0xC1 +#define brERRORCRC 0xC2 +#define brNONE 0xFF + +static union uint8_16u CRC_16; + +static uint8_t cb; +static uint8_t suart_timeout; + +#define WaitStartBitTimeoutms 2 + +#define BitTime (52) //52uS +#define BitHalfTime (BitTime >> 1) //26uS +#define StartBitTime (BitHalfTime + 1) +#define StopBitTime ((BitTime * 9) + BitHalfTime) + + + +static uint8_t suart_getc_(void) +{ + uint8_t bt=0; + uint32_t btime; + uint32_t bstop; + uint32_t start_time; + + suart_timeout = 1; + uint32_t wait_time = millis() + WaitStartBitTimeoutms; + while (ESC_IS_HI) { + // check for Startbit begin + if (millis() >= wait_time) { + return 0; + } + } + // Startbit + start_time = micros(); + + btime = start_time + StartBitTime; + bstop= start_time + StopBitTime; + + while (micros() < btime); + if (ESC_IS_HI) { + return 0; + } + for (uint8_t bit = 0; bit < 8; bit++) + { + btime = btime + BitTime; + while (micros() < btime); + if (ESC_IS_HI) + { + bt |= (1 << bit); + } + } + while (micros() < bstop); + // check Stoppbit + if (ESC_IS_LO) { + return 0; + } + suart_timeout = 0; + return (bt); +} + +static void suart_putc_(uint8_t TXbyte) +{ + uint32_t btime; + ESC_SET_LO; // Set low = StartBit + btime = BitTime + micros(); + while (micros() < btime); + for(uint8_t bit = 0; bit < 8; bit++) + { + if(TXbyte & 1) + { + ESC_SET_HI; // 1 + } + else + { + ESC_SET_LO; // 0 + } + btime = btime + BitTime; + TXbyte = (TXbyte >> 1); + while (micros() < btime); + } + ESC_SET_HI; //Set high = Stoppbit + btime = btime + BitTime; + while (micros() < btime); +} + +static union uint8_16u LastCRC_16; + +static void ByteCrc(void) +{ + for (uint8_t i = 0; i < 8; i++) + { + if (((cb & 0x01) ^ (CRC_16.word & 0x0001)) !=0 ) + { + CRC_16.word = CRC_16.word >> 1; + CRC_16.word = CRC_16.word ^ 0xA001; + } + else + { + CRC_16.word = CRC_16.word >> 1; + } + cb = cb >> 1; + } +} + +static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len) +{ + //Todo CRC in case of timeout? + //len 0 means 256 + CRC_16.word = 0; + LastCRC_16.word = 0; + uint8_t LastACK = brNONE; + do { + cb = suart_getc_(); + if(suart_timeout) goto timeout; + *pstring = cb; + ByteCrc(); + pstring++; + len--; + } while(len > 0); + + if(IsMcuConnected) { + //With CRC read 3 more + LastCRC_16.bytes[0] = suart_getc_(); + if(suart_timeout) goto timeout; + LastCRC_16.bytes[1] = suart_getc_(); + if(suart_timeout) goto timeout; + LastACK = suart_getc_(); + if (CRC_16.word != LastCRC_16.word) { + LastACK = brERRORCRC; + } + } else { + //TODO check here LastACK + LastACK = suart_getc_(); + } +timeout: + return (LastACK == brSUCCESS); +} + +static void BL_SendBuf(uint8_t *pstring, uint8_t len) +{ + ESC_OUTPUT; + // wait some us + delayMicroseconds(50); + CRC_16.word=0; + do { + cb = *pstring; + pstring++; + suart_putc_(cb); + ByteCrc(); + len--; + } while (len > 0); + + if (IsMcuConnected) { + suart_putc_(CRC_16.bytes[0]); + suart_putc_(CRC_16.bytes[1]); + } + ESC_INPUT; +} + +uint8_t BL_ConnectEx(void) +{ + #define BootMsgLen 4 + #define DevSignHi (BootMsgLen) + #define DevSignLo (BootMsgLen+1) + + //DeviceInfo.dword=0; is set before + uint8_t BootInfo[9]; + uint8_t BootMsg[BootMsgLen-1] = "471"; + // x * 0 + 9 +#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + BL_SendBuf(BootInit, 21); +#else + uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + BL_SendBuf(BootInit, 17); +#endif + if (!BL_ReadBuf(BootInfo, BootMsgLen + 4)) { + return 0; + } + // BootInfo has no CRC (ACK byte already analyzed... ) + // Format = BootMsg("471c") SIGNATURE_001, SIGNATURE_002, BootVersion (always 6), BootPages (,ACK) + for (uint8_t i = 0; i < (BootMsgLen - 1); i++) { // Check only the first 3 letters -> 471x OK + if (BootInfo[i] != BootMsg[i]) { + return (0); + } + } + + //only 2 bytes used $1E9307 -> 0x9307 + DeviceInfo.bytes[2] = BootInfo[BootMsgLen - 1]; + DeviceInfo.bytes[1] = BootInfo[DevSignHi]; + DeviceInfo.bytes[0] = BootInfo[DevSignLo]; + return (1); +} + +static uint8_t BL_GetACK(uint32_t Timeout) +{ + uint8_t LastACK; + do { + LastACK = suart_getc_(); + Timeout--; + } while ((suart_timeout) && (Timeout)); + + if(suart_timeout) { + LastACK = brNONE; + } + return (LastACK); +} + + +uint8_t BL_SendCMDKeepAlive(void) +{ + uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0}; + BL_SendBuf(sCMD, 2); + if (BL_GetACK(1) != brERRORCOMMAND) { + return 0; + } + return 1; +} + +void BL_SendCMDRunRestartBootloader(void) +{ + uint8_t sCMD[] = {RestartBootloader, 0}; + DeviceInfo.bytes[0] = 1; + BL_SendBuf(sCMD, 2); //sends simply 4 x 0x00 (CRC =00) + return; +} + +static uint8_t BL_SendCMDSetAddress(void) //supports only 16 bit Adr +{ + // skip if adr == 0xFFFF + if((D_FLASH_ADDR_H == 0xFF) && (D_FLASH_ADDR_L == 0xFF)) return 1; + uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, D_FLASH_ADDR_H, D_FLASH_ADDR_L }; + BL_SendBuf(sCMD, 4); + return (BL_GetACK(2) == brSUCCESS); +} + +static uint8_t BL_SendCMDSetBuffer(void) +{ + uint8_t sCMD[] = {CMD_SET_BUFFER, 0, 0, D_NUM_BYTES}; + if (D_NUM_BYTES == 0) { + // set high byte + sCMD[2] = 1; + } + BL_SendBuf(sCMD, 4); + if (BL_GetACK(2) != brNONE) return 0; + BL_SendBuf(D_PTR_I, D_NUM_BYTES); + return (BL_GetACK(40) == brSUCCESS); +} + +static uint8_t BL_ReadA(uint8_t cmd) +{ + if (BL_SendCMDSetAddress()) { + uint8_t sCMD[] = {cmd, D_NUM_BYTES}; + BL_SendBuf(sCMD, 2); + return (BL_ReadBuf(D_PTR_I, D_NUM_BYTES )); + } + return 0; +} + +static uint8_t BL_WriteA(uint8_t cmd, uint32_t timeout) +{ + if (BL_SendCMDSetAddress()) { + if (!BL_SendCMDSetBuffer()) return 0; + uint8_t sCMD[] = {cmd, 0x01}; + BL_SendBuf(sCMD, 2); + return (BL_GetACK(timeout) == brSUCCESS); + } + return 0; +} + + +uint8_t BL_ReadFlash(uint8_t interface_mode) +{ + if(interface_mode == imATM_BLB) { + return BL_ReadA(CMD_READ_FLASH_ATM); + } else { + return BL_ReadA(CMD_READ_FLASH_SIL); + } +} + + +uint8_t BL_ReadEEprom(void) +{ + return BL_ReadA(CMD_READ_EEPROM); +} + +uint8_t BL_PageErase(void) +{ + if (BL_SendCMDSetAddress()) { + uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01}; + BL_SendBuf(sCMD, 2); + return (BL_GetACK((40 / WaitStartBitTimeoutms)) == brSUCCESS); + } + return 0; +} + +uint8_t BL_WriteEEprom(void) +{ + return BL_WriteA(CMD_PROG_EEPROM, (3000 / WaitStartBitTimeoutms)); +} + +uint8_t BL_WriteFlash(void) +{ + return BL_WriteA(CMD_PROG_FLASH, (40 / WaitStartBitTimeoutms)); +} + +#endif diff --git a/src/main/io/serial_4way_avrootloader.h b/src/main/io/serial_4way_avrootloader.h new file mode 100644 index 000000000..6c4f4d143 --- /dev/null +++ b/src/main/io/serial_4way_avrootloader.h @@ -0,0 +1,34 @@ +/* + * 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 . + * Author: 4712 + * for info about Hagens AVRootloader: + * http://www.mikrocontroller.net/topic/avr-bootloader-mit-verschluesselung +*/ + +#pragma once + +#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) + +void BL_SendBootInit(void); +uint8_t BL_ConnectEx(void); +uint8_t BL_SendCMDKeepAlive(void); +uint8_t BL_PageErase(void); +uint8_t BL_ReadEEprom(void); +uint8_t BL_WriteEEprom(void); +uint8_t BL_WriteFlash(void); +uint8_t BL_ReadFlash(uint8_t interface_mode); +void BL_SendCMDRunRestartBootloader(void); +#endif diff --git a/src/main/io/serial_4way_stk500v2.c b/src/main/io/serial_4way_stk500v2.c new file mode 100644 index 000000000..2bd0c6f7c --- /dev/null +++ b/src/main/io/serial_4way_stk500v2.c @@ -0,0 +1,430 @@ +/* + * 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 . + * Author: 4712 + * have a look at https://github.com/sim-/tgy/blob/master/boot.inc + * for info about the stk500v2 implementation + */ +#include +#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER +#include +#include +#include +#include +#include "io/serial_4way_stk500v2.h" +#include "drivers/system.h" +#include "io/serial_4way.h" +#include "config/config.h" + +#define BIT_LO_US (32) //32uS +#define BIT_HI_US (2*BIT_LO_US) + +static uint8_t StkInBuf[16]; + +#define STK_BIT_TIMEOUT 250 // micro seconds +#define STK_WAIT_TICKS (1000 / STK_BIT_TIMEOUT) // per ms +#define STK_WAITCYLCES (STK_WAIT_TICKS * 35) // 35ms +#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5 ms +#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) //5 s + +#define WaitPinLo while (ESC_IS_HI) {if (micros() > timeout_timer) goto timeout;} +#define WaitPinHi while (ESC_IS_LO) {if (micros() > timeout_timer) goto timeout;} + +static uint32_t LastBitTime; +static uint32_t HiLoTsh; + +static uint8_t SeqNumber; +static uint8_t StkCmd; +static uint8_t ckSumIn; +static uint8_t ckSumOut; + +// used STK message constants from ATMEL AVR - Application note +#define MESSAGE_START 0x1B +#define TOKEN 0x0E + +#define CMD_SIGN_ON 0x01 +#define CMD_LOAD_ADDRESS 0x06 +#define CMD_CHIP_ERASE_ISP 0x12 +#define CMD_PROGRAM_FLASH_ISP 0x13 +#define CMD_READ_FLASH_ISP 0x14 +#define CMD_PROGRAM_EEPROM_ISP 0x15 +#define CMD_READ_EEPROM_ISP 0x16 +#define CMD_READ_SIGNATURE_ISP 0x1B +#define CMD_SPI_MULTI 0x1D + +#define STATUS_CMD_OK 0x00 + +#define CmdFlashEepromRead 0xA0 +#define EnterIspCmd1 0xAC +#define EnterIspCmd2 0x53 +#define signature_r 0x30 + +#define delay_us(x) delayMicroseconds(x) +#define IRQ_OFF // dummy +#define IRQ_ON // dummy + +static void StkSendByte(uint8_t dat) +{ + ckSumOut ^= dat; + for (uint8_t i = 0; i < 8; i++) + { + if (dat & 0x01) + { + // 1-bits are encoded as 64.0us high, 72.8us low (135.8us total). + ESC_SET_HI; + delay_us(BIT_HI_US); + ESC_SET_LO; + delay_us(BIT_HI_US); + } + else + { + // 0-bits are encoded as 27.8us high, 34.5us low, 34.4us high, 37.9 low (134.6us total) + ESC_SET_HI; + delay_us(BIT_LO_US); + ESC_SET_LO; + delay_us(BIT_LO_US); + ESC_SET_HI; + delay_us(BIT_LO_US); + ESC_SET_LO; + delay_us(BIT_LO_US); + } + dat >>= 1; + } +} + +static void StkSendPacketHeader(void) +{ + IRQ_OFF; + ESC_OUTPUT; + StkSendByte(0xFF); + StkSendByte(0xFF); + StkSendByte(0x7F); + ckSumOut=0; + StkSendByte(MESSAGE_START); + StkSendByte(++SeqNumber); +} + +static void StkSendPacketFooter(void) +{ + StkSendByte(ckSumOut); + ESC_SET_HI; + delay_us(BIT_LO_US); + ESC_INPUT; + IRQ_ON; +} + + + +static int8_t ReadBit(void) +{ + uint32_t btimer = micros(); + uint32_t timeout_timer = btimer + STK_BIT_TIMEOUT; + WaitPinLo; + WaitPinHi; + LastBitTime = micros() - btimer; + if (LastBitTime <= HiLoTsh) + { + timeout_timer = timeout_timer + STK_BIT_TIMEOUT; + WaitPinLo; + WaitPinHi; + //lo-bit + return 0; + } + else return 1; +timeout: + return -1; +} + +static uint8_t ReadByte(uint8_t *bt) +{ + *bt = 0; + for (uint8_t i = 0; i < 8; i++) + { + int8_t bit = ReadBit(); + if (bit == -1) goto timeout; + if (bit == 1) { + *bt |= (1 << i); + } + } + ckSumIn ^=*bt; + return 1; +timeout: + return 0; +} + + + +static uint8_t StkReadLeader(void) +{ + + // Reset learned timing + HiLoTsh = BIT_HI_US + BIT_LO_US; + + // Wait for the first bit + uint32_t waitcycl; //250uS each + + if((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) + { + waitcycl = STK_WAITCYLCES_EXT; + } + else if(StkCmd == CMD_SIGN_ON) + { + waitcycl = STK_WAITCYLCES_START; + } + else + { + waitcycl= STK_WAITCYLCES; + } + for ( ; waitcycl >0 ; waitcycl--) + { + //check is not timeout + if (ReadBit() >- 1) break; + } + + //Skip the first bits + if (waitcycl == 0) + { + goto timeout; + } + + for (uint8_t i = 0; i < 10; i++) + { + if (ReadBit() == -1) goto timeout; + } + + // learn timing + HiLoTsh = (LastBitTime >> 1) + (LastBitTime >> 2); + + // Read until we get a 0 bit + int8_t bit; + do + { + bit = ReadBit(); + if (bit == -1) goto timeout; + } while (bit > 0); + return 1; +timeout: + return 0; +} + +static uint8_t StkRcvPacket(uint8_t *pstring) +{ + uint8_t bt = 0; + union uint8_16u Len; + + IRQ_OFF; + if (!StkReadLeader()) goto Err; + ckSumIn=0; + if (!ReadByte(&bt) || (bt != MESSAGE_START)) goto Err; + if (!ReadByte(&bt) || (bt != SeqNumber)) goto Err; + ReadByte(&Len.bytes[1]); + if (Len.bytes[1] > 1) goto Err; + ReadByte(&Len.bytes[0]); + if (Len.bytes[0] < 1) goto Err; + if (!ReadByte(&bt) || (bt != TOKEN)) goto Err; + if (!ReadByte(&bt) || (bt != StkCmd)) goto Err; + if (!ReadByte(&bt) || (bt != STATUS_CMD_OK)) goto Err; + for (uint16_t i = 0; i < (Len.word - 2); i++) + { + if (!ReadByte(pstring)) goto Err; + pstring++; + } + ReadByte(&bt); + if (ckSumIn != 0) goto Err; + IRQ_ON; + return 1; +Err: + IRQ_ON; + return 0; +} + +static uint8_t _CMD_SPI_MULTI_EX(volatile uint8_t * ResByte,uint8_t Cmd,uint8_t AdrHi,uint8_t AdrLo) +{ + StkCmd= CMD_SPI_MULTI; + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(8); // lo byte Msg len + StkSendByte(TOKEN); + StkSendByte(CMD_SPI_MULTI); + StkSendByte(4); // NumTX + StkSendByte(4); // NumRX + StkSendByte(0); // RxStartAdr + StkSendByte(Cmd); // {TxData} Cmd + StkSendByte(AdrHi); // {TxData} AdrHi + StkSendByte(AdrLo); // {TxData} AdrLoch + StkSendByte(0); // {TxData} 0 + StkSendPacketFooter(); + if (StkRcvPacket((void *)StkInBuf)) { // NumRX + 3 + if ((StkInBuf[0] == 0x00) && ((StkInBuf[1] == Cmd)||(StkInBuf[1] == 0x00)/* ignore zero returns */) &&(StkInBuf[2] == 0x00)) { + *ResByte = StkInBuf[3]; + } + return 1; + } + return 0; +} + +static uint8_t _CMD_LOAD_ADDRESS(void) +{ + // ignore 0xFFFF + // assume address is set before and we read or write the immediately following package + if((D_FLASH_ADDR_H == 0xFF) && (D_FLASH_ADDR_L == 0xFF)) return 1; + StkCmd = CMD_LOAD_ADDRESS; + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(5); // lo byte Msg len + StkSendByte(TOKEN); + StkSendByte(CMD_LOAD_ADDRESS); + StkSendByte(0); + StkSendByte(0); + StkSendByte(D_FLASH_ADDR_H); + StkSendByte(D_FLASH_ADDR_L); + StkSendPacketFooter(); + return (StkRcvPacket((void *)StkInBuf)); +} + +static uint8_t _CMD_READ_MEM_ISP() +{ + uint8_t LenHi; + if (D_NUM_BYTES>0) { + LenHi=0; + } else { + LenHi=1; + } + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(4); // lo byte Msg len + StkSendByte(TOKEN); + StkSendByte(StkCmd); + StkSendByte(LenHi); + StkSendByte(D_NUM_BYTES); + StkSendByte(CmdFlashEepromRead); + StkSendPacketFooter(); + return (StkRcvPacket(D_PTR_I)); +} + +static uint8_t _CMD_PROGRAM_MEM_ISP(void) +{ + union uint8_16u Len; + uint8_t LenLo=D_NUM_BYTES; + uint8_t LenHi; + if (LenLo) { + LenHi=0; + Len.word=LenLo+10; + } else { + LenHi=1; + Len.word=256+10; + } + StkSendPacketHeader(); + StkSendByte(Len.bytes[1]); // high byte Msg len + StkSendByte(Len.bytes[0]); // low byte Msg len + StkSendByte(TOKEN); + StkSendByte(StkCmd); + StkSendByte(LenHi); + StkSendByte(LenLo); + StkSendByte(0); // mode + StkSendByte(0); // delay + StkSendByte(0); // cmd1 + StkSendByte(0); // cmd2 + StkSendByte(0); // cmd3 + StkSendByte(0); // poll1 + StkSendByte(0); // poll2 + do { + StkSendByte(*D_PTR_I); + D_PTR_I++; + LenLo--; + } while (LenLo); + StkSendPacketFooter(); + return StkRcvPacket((void *)StkInBuf); +} + +uint8_t Stk_SignOn(void) +{ + StkCmd=CMD_SIGN_ON; + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(1); // lo byte Msg len + StkSendByte(TOKEN); + StkSendByte(CMD_SIGN_ON); + StkSendPacketFooter(); + return (StkRcvPacket((void *) StkInBuf)); +} + +uint8_t Stk_ConnectEx(void) +{ + if (Stk_SignOn()) { + if (_CMD_SPI_MULTI_EX(&DeviceInfo.bytes[1],signature_r,0,1)) { + if (_CMD_SPI_MULTI_EX(&DeviceInfo.bytes[0],signature_r,0,2)) { + return 1; + } + } + } + return 0; +} + +uint8_t Stk_Chip_Erase(void) +{ + StkCmd = CMD_CHIP_ERASE_ISP; + StkSendPacketHeader(); + StkSendByte(0); // high byte Msg len + StkSendByte(7); // low byte Msg len + StkSendByte(TOKEN); + StkSendByte(CMD_CHIP_ERASE_ISP); + StkSendByte(20); // ChipErase_eraseDelay atmega8 + StkSendByte(0); // ChipErase_pollMethod atmega8 + StkSendByte(0xAC); + StkSendByte(0x88); + StkSendByte(0x13); + StkSendByte(0x76); + StkSendPacketFooter(); + return (StkRcvPacket((void *)StkInBuf)); +} + +uint8_t Stk_ReadFlash(void) +{ + if (_CMD_LOAD_ADDRESS()) { + StkCmd = CMD_READ_FLASH_ISP; + return (_CMD_READ_MEM_ISP()); + } + return 0; +} + + +uint8_t Stk_ReadEEprom(void) +{ + if (_CMD_LOAD_ADDRESS()) { + StkCmd = CMD_READ_EEPROM_ISP; + return (_CMD_READ_MEM_ISP()); + } + return 0; +} + +uint8_t Stk_WriteFlash(void) +{ + if (_CMD_LOAD_ADDRESS()) { + StkCmd = CMD_PROGRAM_FLASH_ISP; + return (_CMD_PROGRAM_MEM_ISP()); + } + return 0; +} + +uint8_t Stk_WriteEEprom(void) +{ + if (_CMD_LOAD_ADDRESS()) { + StkCmd = CMD_PROGRAM_EEPROM_ISP; + return (_CMD_PROGRAM_MEM_ISP()); + } + return 0; +} +#endif diff --git a/src/main/io/serial_4way_stk500v2.h b/src/main/io/serial_4way_stk500v2.h new file mode 100644 index 000000000..f24b60ae7 --- /dev/null +++ b/src/main/io/serial_4way_stk500v2.h @@ -0,0 +1,31 @@ +/* + * 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 . + * Author: 4712 +*/ +#include +#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + +#pragma once + +uint8_t Stk_SignOn(void); +uint8_t Stk_ConnectEx(void); +uint8_t Stk_ReadEEprom(void); +uint8_t Stk_WriteEEprom(void); +uint8_t Stk_ReadFlash(void); +uint8_t Stk_WriteFlash(void); +uint8_t Stk_Chip_Erase(void); + +#endif diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 3bc83fd93..2b2c6269f 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -99,6 +99,10 @@ #ifdef USE_SERIAL_1WIRE_VCP #include "io/serial_1wire_vcp.h" #endif +#if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) +#include "io/serial_4way.h" +#endif + #ifdef USE_ESCSERIAL #include "drivers/serial_escserial.h" #endif @@ -1854,6 +1858,28 @@ static bool processInCommand(void) } break; #endif +#if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) + case MSP_SET_4WAY_IF: + // get channel number + // switch all motor lines HI + // reply the count of ESC found + headSerialReply(1); + serialize8(Initialize4WayInterface()); + // because we do not come back after calling Process4WayInterface + // proceed with a success reply first + tailSerialReply(); + // flush the transmit buffer + bufWriterFlush(writer); + // wait for all data to send + waitForSerialPortToFinishTransmitting(currentPort->port); + // rem: App: Wait at least appx. 500 ms for BLHeli to jump into + // bootloader mode before try to connect any ESC + // Start to activate here + Process4WayInterface(currentPort, writer); + // former used MSP uart is still active + // proceed as usual with MSP commands + break; +#endif #ifdef USE_ESCSERIAL case MSP_SET_ESCSERIAL: diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 053a18691..fe9d9b9e1 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -256,6 +256,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #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 +#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface // 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/CC3D/target.h b/src/main/target/CC3D/target.h index cc6dc885c..715c588f5 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -126,11 +126,16 @@ #define USE_SERVOS #define USE_CLI +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) #ifdef USE_VCP #define USE_SERIAL_1WIRE_VCP #else #define USE_SERIAL_1WIRE #endif +#endif #ifdef USE_SERIAL_1WIRE // FlexPort (pin 21/22, TX/RX respectively): diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index add19d9d7..87758ab4a 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -184,11 +184,17 @@ #define USE_SERVOS #define USE_CLI +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) #ifdef USE_VCP #define USE_SERIAL_1WIRE_VCP #else #define USE_SERIAL_1WIRE #endif +#endif + #ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO GPIOB #define S1W_TX_PIN GPIO_Pin_10 diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index d7ca4e76b..9a2077af0 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -165,11 +165,17 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_11 +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) #ifdef USE_VCP #define USE_SERIAL_1WIRE_VCP #else #define USE_SERIAL_1WIRE #endif +#endif + #ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO GPIOA #define S1W_TX_PIN GPIO_Pin_9 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index f8c87d3e5..13aeb63c1 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -165,11 +165,17 @@ #define BIND_PORT GPIOC #define BIND_PIN Pin_5 +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) #ifdef USE_VCP #define USE_SERIAL_1WIRE_VCP #else #define USE_SERIAL_1WIRE #endif +#endif + #ifdef USE_SERIAL_1WIRE // Untested #define S1W_TX_GPIO GPIOB diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index a123d5325..97bd1d805 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -191,11 +191,16 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_4 +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) #ifdef USE_VCP #define USE_SERIAL_1WIRE_VCP #else #define USE_SERIAL_1WIRE #endif +#endif #ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO GPIOB diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index de468e0b6..701d9e2da 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -191,11 +191,16 @@ #define BIND_PORT GPIOA #define BIND_PIN Pin_3 +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) #ifdef USE_VCP #define USE_SERIAL_1WIRE_VCP #else #define USE_SERIAL_1WIRE #endif +#endif #ifdef USE_SERIAL_1WIRE // STM32F103CBT6-LQFP48 Pin30 (PA9) TX - PC3 connects to onboard CP2102 RX diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 64436374f..7ed70db57 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -156,11 +156,17 @@ #define TELEMETRY #define USE_SERVOS #define USE_CLI + +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) #ifdef USE_VCP #define USE_SERIAL_1WIRE_VCP #else #define USE_SERIAL_1WIRE #endif +#endif #ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO GPIOA diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index f8b72d1d8..6fbd16a36 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -157,11 +157,16 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_11 +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) #ifdef USE_VCP #define USE_SERIAL_1WIRE_VCP #else #define USE_SERIAL_1WIRE #endif +#endif #ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO GPIOA diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index d727f8c5b..7a2ba579a 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -166,11 +166,16 @@ #define WS2811_IRQ DMA1_Channel7_IRQn #endif +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) #ifdef USE_VCP #define USE_SERIAL_1WIRE_VCP #else #define USE_SERIAL_1WIRE #endif +#endif #ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO GPIOB diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 08937aac7..b83f65e7e 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -165,11 +165,16 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_11 +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) #ifdef USE_VCP #define USE_SERIAL_1WIRE_VCP #else #define USE_SERIAL_1WIRE #endif +#endif #ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO GPIOA diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 08d6bf7c9..e843edd03 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -234,11 +234,16 @@ #define BINDPLUG_PORT BUTTON_B_PORT #define BINDPLUG_PIN BUTTON_B_PIN +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) #ifdef USE_VCP #define USE_SERIAL_1WIRE_VCP #else #define USE_SERIAL_1WIRE #endif +#endif #ifdef USE_SERIAL_1WIRE #define S1W_TX_GPIO UART1_GPIO diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 0b8e1bb84..e474ef073 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -162,11 +162,16 @@ #define USE_SERVOS #define USE_CLI +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) #ifdef USE_VCP #define USE_SERIAL_1WIRE_VCP #else #define USE_SERIAL_1WIRE #endif +#endif #ifdef USE_SERIAL_1WIRE // STM32F3DISCOVERY TX - PD5 connects to UART RX From 3a19b9dc8ec64b624c7bb2f2fdd7d3f7a6b2f8fe Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Sun, 3 Apr 2016 08:46:39 +0200 Subject: [PATCH 26/78] Output valid range/values for paramter --- src/main/io/serial_cli.c | 28 ++++++++++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 572f4ffcb..f89955202 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -461,7 +461,7 @@ typedef enum { TABLE_GPS_SBAS_MODE, #endif #ifdef BLACKBOX - TABLE_BLACKBOX_DEVICE, + TABLE_BLACKBOX_DEVICE, #endif TABLE_CURRENT_SENSOR, TABLE_GIMBAL_MODE, @@ -781,6 +781,7 @@ typedef union { static void cliSetVar(const clivalue_t *var, const int_float_value_t value); static void cliPrintVar(const clivalue_t *var, uint32_t full); +static void cliPrintVarRange(const clivalue_t *var); static void cliPrint(const char *str); static void cliPrintf(const char *fmt, ...); static void cliWrite(uint8_t ch); @@ -2557,7 +2558,27 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full) break; } } - +static void cliPrintVarRange(const clivalue_t *var) +{ + switch (var->type & VALUE_MODE_MASK) { + case (MODE_DIRECT): { + cliPrintf("Allowed range: %d - %d\n", var->config.minmax.min, var->config.minmax.max); + } + break; + case (MODE_LOOKUP): { + const lookupTableEntry_t *tableEntry = &lookupTables[var->config.lookup.tableIndex]; + cliPrint("Allowed values:"); + uint8_t i; + for (i = 0; i < tableEntry->valueCount ; i++) { + if (i > 0) + cliPrint(","); + cliPrintf(" %s", tableEntry->values[i]); + } + cliPrint("\n"); + } + break; + } +} static void cliSetVar(const clivalue_t *var, const int_float_value_t value) { void *ptr = var->ptr; @@ -2669,6 +2690,7 @@ static void cliSet(char *cmdline) cliPrintVar(val, 0); } else { cliPrint("Invalid value\r\n"); + cliPrintVarRange(val); } return; @@ -2692,6 +2714,8 @@ static void cliGet(char *cmdline) val = &valueTable[i]; cliPrintf("%s = ", valueTable[i].name); cliPrintVar(val, 0); + cliPrint("\n"); + cliPrintVarRange(val); cliPrint("\r\n"); matchedCommands++; From 272932a93ff1213bfc3f4a56513584d7c11a89df Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 4 Apr 2016 01:37:20 +0200 Subject: [PATCH 27/78] Revert "Add escpassthrough to all targets" This reverts commit 95a464b1ad9b70c5b8fe6dbf9712fb908e75eb36. --- Makefile | 2 +- src/main/io/serial_cli.c | 1 - src/main/io/serial_msp.c | 1 - src/main/target/ALIENFLIGHTF3/target.h | 3 --- src/main/target/CC3D/target.h | 2 -- src/main/target/IRCFUSIONF3/target.h | 3 --- src/main/target/LUX_RACE/target.h | 3 --- src/main/target/MOTOLAB/target.h | 3 --- src/main/target/NAZE/target.h | 3 --- src/main/target/RMDO/target.h | 3 --- src/main/target/SPARKY/target.h | 3 --- src/main/target/SPRACINGF3/target.h | 2 -- src/main/target/SPRACINGF3MINI/target.h | 2 -- 13 files changed, 1 insertion(+), 30 deletions(-) diff --git a/Makefile b/Makefile index b5ff16703..93b38754a 100644 --- a/Makefile +++ b/Makefile @@ -299,7 +299,6 @@ COMMON_SRC = build_config.c \ drivers/gyro_sync.c \ drivers/dma.c \ drivers/buf_writer.c \ - drivers/serial_escserial.c \ io/beeper.c \ io/rc_controls.c \ io/rc_curves.c \ @@ -609,6 +608,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/io/serial_cli.c b/src/main/io/serial_cli.c index f89955202..089d1e89b 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -47,7 +47,6 @@ #include "drivers/pwm_rx.h" #include "drivers/sdcard.h" #include "drivers/gyro_sync.h" -#include "drivers/serial_escserial.h" #include "drivers/buf_writer.h" diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 2b2c6269f..cbd629fbf 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -44,7 +44,6 @@ #include "drivers/gyro_sync.h" #include "drivers/sdcard.h" #include "drivers/buf_writer.h" -#include "drivers/serial_escserial.h" #include "rx/rx.h" #include "rx/msp.h" diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index ca89307f7..79365c62e 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -51,9 +51,6 @@ //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL -#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 - // Using MPU6050 for the moment. #define GYRO #define USE_GYRO_MPU6050 diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 715c588f5..c3639e4d9 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -27,8 +27,6 @@ #define INVERTER_PERIPHERAL RCC_APB2Periph_GPIOB #define INVERTER_USART USART1 -#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define BEEP_GPIO GPIOA #define BEEP_PIN Pin_15 // PA15 (Beeper) diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 9a2077af0..ccc9fa997 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -38,9 +38,6 @@ #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH -#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 - #define GYRO #define USE_GYRO_MPU6050 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 13aeb63c1..3b19243aa 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -58,9 +58,6 @@ #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready -#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 - #define GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 97bd1d805..a7961aa4c 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -35,9 +35,6 @@ #define USABLE_TIMER_CHANNEL_COUNT 9 -#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 - // MPU6050 interrupts #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 701d9e2da..239d03c66 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -29,9 +29,6 @@ #define LED1_PIN Pin_4 // PB4 (LED) #define LED1_PERIPHERAL RCC_APB2Periph_GPIOB -#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 - #define BEEP_GPIO GPIOA #define BEEP_PIN Pin_12 // PA12 (Beeper) #define BEEP_PERIPHERAL RCC_APB2Periph_GPIOA diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 6fbd16a36..b17efe35e 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -28,9 +28,6 @@ #define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC #define BEEPER_INVERTED -#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 - #define USABLE_TIMER_CHANNEL_COUNT 17 #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 7a2ba579a..7f85e4d73 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -34,9 +34,6 @@ #define USABLE_TIMER_CHANNEL_COUNT 11 -#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 - // MPU6050 interrupts #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index b83f65e7e..29c2d216b 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -38,8 +38,6 @@ #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH -#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define GYRO #define USE_GYRO_MPU6050 diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index e843edd03..e9780b134 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -41,8 +41,6 @@ #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH -#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define GYRO //#define USE_FAKE_GYRO From 26af73051024242cc3fda5d696770b226ae83ab3 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 4 Apr 2016 01:42:13 +0200 Subject: [PATCH 28/78] Revert "- MultiFlasher support for SimonK escs" This reverts commit 0cae020f768ef12deb59803c3638d84c0eb34854. --- 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 | 48 +- src/main/io/serial_msp.h | 1 - src/main/target/COLIBRI_RACE/target.h | 3 - 7 files changed, 1 insertion(+), 1032 deletions(-) delete mode 100755 src/main/drivers/serial_escserial.c delete mode 100755 src/main/drivers/serial_escserial.h diff --git a/Makefile b/Makefile index 93b38754a..e4dc3b51a 100644 --- a/Makefile +++ b/Makefile @@ -608,7 +608,6 @@ 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 deleted file mode 100755 index fe260bae5..000000000 --- a/src/main/drivers/serial_escserial.c +++ /dev/null @@ -1,888 +0,0 @@ -/* - * 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 deleted file mode 100755 index ab4a1abd3..000000000 --- a/src/main/drivers/serial_escserial.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * 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 089d1e89b..1976423cc 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -141,9 +141,6 @@ 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); @@ -274,9 +271,6 @@ 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 @@ -2201,56 +2195,6 @@ 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 cbd629fbf..e3ec3579e 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -95,6 +95,7 @@ #ifdef USE_SERIAL_1WIRE #include "io/serial_1wire.h" #endif + #ifdef USE_SERIAL_1WIRE_VCP #include "io/serial_1wire_vcp.h" #endif @@ -102,9 +103,6 @@ #include "io/serial_4way.h" #endif -#ifdef USE_ESCSERIAL -#include "drivers/serial_escserial.h" -#endif static serialPort_t *mspSerialPort; extern uint16_t cycleTime; // FIXME dependency on mw.c @@ -1879,50 +1877,6 @@ static bool processInCommand(void) // proceed as usual with MSP commands 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 fe9d9b9e1..c493e37f1 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -255,7 +255,6 @@ 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 #define MSP_SET_4WAY_IF 245 //in message Sets 4way interface // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports. diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 87758ab4a..c618dd14a 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -110,9 +110,6 @@ #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) From 1afe6a6cafa7315ff78fa7920f62b098b4a9ca44 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 4 Apr 2016 01:42:50 +0200 Subject: [PATCH 29/78] Bump Version --- src/main/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/version.h b/src/main/version.h index c79b4db7a..799c7d43b 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -17,7 +17,7 @@ #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed +#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) From eb9c8a22b067cb63d0780d3313d82019e14d5274 Mon Sep 17 00:00:00 2001 From: 4712 <4712@outlook.de> Date: Mon, 4 Apr 2016 20:33:15 +0200 Subject: [PATCH 30/78] 4way-interface cleanup A few optimizations as well --- src/main/io/serial_1wire_vcp.c | 69 +++++------ src/main/io/serial_4way.c | 2 - src/main/io/serial_4way_avrootloader.c | 165 ++++++++++--------------- src/main/io/serial_4way_avrootloader.h | 2 +- 4 files changed, 94 insertions(+), 144 deletions(-) diff --git a/src/main/io/serial_1wire_vcp.c b/src/main/io/serial_1wire_vcp.c index db93b4695..6e350f769 100644 --- a/src/main/io/serial_1wire_vcp.c +++ b/src/main/io/serial_1wire_vcp.c @@ -104,70 +104,57 @@ void usb1WireDeInitializeVcp(void){ #define START_BIT_TIME (BIT_TIME_HALVE + 1) #define STOP_BIT_TIME ((BIT_TIME * 9) + BIT_TIME_HALVE) -static void suart_putc_(uint8_t tx_b) +static void suart_putc_(uint8_t *tx_b) { - uint32_t btime; - ESC_SET_LO; // 0 = start bit - btime = BIT_TIME + micros(); - while (micros() < btime); - for(uint8_t bit = 0; bit <8; bit++) - { - if(tx_b & 1) - { + // shift out stopbit first + uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10); + uint32_t btime = micros(); + while(1) { + if(bitmask & 1) { ESC_SET_HI; // 1 - } - else - { + } else { ESC_SET_LO; // 0 } btime = btime + BIT_TIME; - tx_b = (tx_b >> 1); + bitmask = (bitmask >> 1); + if (bitmask == 0) break; // stopbit shifted out - but don't wait while (micros() < btime); } - ESC_SET_HI; // 1 = stop bit - btime = btime + BIT_TIME; - while (micros() < btime); } - static uint8_t suart_getc_(uint8_t *bt) { uint32_t btime; uint32_t start_time; - uint32_t stop_time; - uint32_t wait_start; - *bt = 0; - - wait_start = millis() + START_BIT_TIMEOUT_MS; + uint32_t wait_time = millis() + START_BIT_TIMEOUT_MS; while (ESC_IS_HI) { - // check for start bit begin - if (millis() > wait_start) { + // check for startbit begin + if (millis() >= wait_time) { return 0; } } // start bit start_time = micros(); btime = start_time + START_BIT_TIME; - stop_time = start_time + STOP_BIT_TIME; - + uint16_t bitmask = 0; + uint8_t bit = 0; while (micros() < btime); - - if (ESC_IS_HI) return 0; // check start bit - for (uint8_t bit=0;bit<8;bit++) - { - btime = btime + BIT_TIME; - while (micros() < btime); - if (ESC_IS_HI) - { - *bt |= (1 << bit); // 1 else 0 + while(1) { + if (ESC_IS_HI) { + bitmask |= (1 << bit); } + btime = btime + BIT_TIME; + bit++; + if (bit == 10) break; + while (micros() < btime); } - while (micros() < stop_time); - - if (ESC_IS_LO) return 0; // check stop bit - - return 1; // OK + // check start bit and stop bit + if ((bitmask & 1) || (!(bitmask & (1 << 9)))) { + return 0; + } + *bt = bitmask >> 1; + return 1; } #define USE_TXRX_LED @@ -224,7 +211,7 @@ void usb1WirePassthroughVcp(mspPort_t *mspPort, bufWriter_t *bufwriter, uint8_t TX_LED_ON; do { bt = serialRead(mspPort->port); - suart_putc_(bt); + suart_putc_(&bt); } while(serialRxBytesWaiting(mspPort->port)); ESC_INPUT; TX_LED_OFF; diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 4be6c1364..0e7fc889f 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -358,7 +358,6 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { #ifdef BEEPER // fix for buzzer often starts beeping continuously when the ESCs are read // switch beeper silent here - // TODO (4712) check: is beeperSilence useful here? beeperSilence(); #endif bool isExitScheduled = false; @@ -409,7 +408,6 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { // ******* Interface related stuff ******* case cmd_InterfaceTestAlive: { - break; if (IsMcuConnected){ switch(CurrentInterfaceMode) { diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index a2c2f6890..9aca1fb2e 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -57,136 +57,110 @@ #define brERRORCRC 0xC2 #define brNONE 0xFF -static union uint8_16u CRC_16; -static uint8_t cb; -static uint8_t suart_timeout; +#define START_BIT_TIMEOUT_MS 2 -#define WaitStartBitTimeoutms 2 +#define BIT_TIME (52) //52uS +#define BIT_TIME_HALVE (BIT_TIME >> 1) //26uS +#define START_BIT_TIME (BIT_TIME_HALVE)// + 1) +//#define STOP_BIT_TIME ((BIT_TIME * 9) + BIT_TIME_HALVE) -#define BitTime (52) //52uS -#define BitHalfTime (BitTime >> 1) //26uS -#define StartBitTime (BitHalfTime + 1) -#define StopBitTime ((BitTime * 9) + BitHalfTime) - - - -static uint8_t suart_getc_(void) +static uint8_t suart_getc_(uint8_t *bt) { - uint8_t bt=0; uint32_t btime; - uint32_t bstop; uint32_t start_time; - suart_timeout = 1; - uint32_t wait_time = millis() + WaitStartBitTimeoutms; + uint32_t wait_time = millis() + START_BIT_TIMEOUT_MS; while (ESC_IS_HI) { - // check for Startbit begin + // check for startbit begin if (millis() >= wait_time) { return 0; } } - // Startbit + // start bit start_time = micros(); - - btime = start_time + StartBitTime; - bstop= start_time + StopBitTime; - + btime = start_time + START_BIT_TIME; + uint16_t bitmask = 0; + uint8_t bit = 0; while (micros() < btime); - if (ESC_IS_HI) { - return 0; - } - for (uint8_t bit = 0; bit < 8; bit++) - { - btime = btime + BitTime; - while (micros() < btime); + while(1) { if (ESC_IS_HI) { - bt |= (1 << bit); + bitmask |= (1 << bit); } - } - while (micros() < bstop); - // check Stoppbit - if (ESC_IS_LO) { - return 0; - } - suart_timeout = 0; - return (bt); -} - -static void suart_putc_(uint8_t TXbyte) -{ - uint32_t btime; - ESC_SET_LO; // Set low = StartBit - btime = BitTime + micros(); - while (micros() < btime); - for(uint8_t bit = 0; bit < 8; bit++) - { - if(TXbyte & 1) - { - ESC_SET_HI; // 1 - } - else - { - ESC_SET_LO; // 0 - } - btime = btime + BitTime; - TXbyte = (TXbyte >> 1); + btime = btime + BIT_TIME; + bit++; + if (bit == 10) break; while (micros() < btime); } - ESC_SET_HI; //Set high = Stoppbit - btime = btime + BitTime; - while (micros() < btime); + // check start bit and stop bit + if ((bitmask & 1) || (!(bitmask & (1 << 9)))) { + return 0; + } + *bt = bitmask >> 1; + return 1; } +static void suart_putc_(uint8_t *tx_b) +{ + // shift out stopbit first + uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10); + uint32_t btime = micros(); + while(1) { + if(bitmask & 1) { + ESC_SET_HI; // 1 + } + else { + ESC_SET_LO; // 0 + } + btime = btime + BIT_TIME; + bitmask = (bitmask >> 1); + if (bitmask == 0) break; // stopbit shifted out - but don't wait + while (micros() < btime); + } +} + +static union uint8_16u CRC_16; static union uint8_16u LastCRC_16; -static void ByteCrc(void) +static void ByteCrc(uint8_t *bt) { + uint8_t xb = *bt; for (uint8_t i = 0; i < 8; i++) { - if (((cb & 0x01) ^ (CRC_16.word & 0x0001)) !=0 ) - { + if (((xb & 0x01) ^ (CRC_16.word & 0x0001)) !=0 ) { CRC_16.word = CRC_16.word >> 1; CRC_16.word = CRC_16.word ^ 0xA001; - } - else - { + } else { CRC_16.word = CRC_16.word >> 1; } - cb = cb >> 1; + xb = xb >> 1; } } static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len) { - //Todo CRC in case of timeout? - //len 0 means 256 + // len 0 means 256 CRC_16.word = 0; LastCRC_16.word = 0; uint8_t LastACK = brNONE; do { - cb = suart_getc_(); - if(suart_timeout) goto timeout; - *pstring = cb; - ByteCrc(); + if(!suart_getc_(pstring)) goto timeout; + ByteCrc(pstring); pstring++; len--; } while(len > 0); if(IsMcuConnected) { //With CRC read 3 more - LastCRC_16.bytes[0] = suart_getc_(); - if(suart_timeout) goto timeout; - LastCRC_16.bytes[1] = suart_getc_(); - if(suart_timeout) goto timeout; - LastACK = suart_getc_(); + if(!suart_getc_(&LastCRC_16.bytes[0])) goto timeout; + if(!suart_getc_(&LastCRC_16.bytes[1])) goto timeout; + if(!suart_getc_(&LastACK)) goto timeout; if (CRC_16.word != LastCRC_16.word) { LastACK = brERRORCRC; } } else { - //TODO check here LastACK - LastACK = suart_getc_(); + if(!suart_getc_(&LastACK)) goto timeout; } timeout: return (LastACK == brSUCCESS); @@ -195,20 +169,17 @@ timeout: static void BL_SendBuf(uint8_t *pstring, uint8_t len) { ESC_OUTPUT; - // wait some us - delayMicroseconds(50); CRC_16.word=0; do { - cb = *pstring; + suart_putc_(pstring); + ByteCrc(pstring); pstring++; - suart_putc_(cb); - ByteCrc(); len--; } while (len > 0); if (IsMcuConnected) { - suart_putc_(CRC_16.bytes[0]); - suart_putc_(CRC_16.bytes[1]); + suart_putc_(&CRC_16.bytes[0]); + suart_putc_(&CRC_16.bytes[1]); } ESC_INPUT; } @@ -250,19 +221,13 @@ uint8_t BL_ConnectEx(void) static uint8_t BL_GetACK(uint32_t Timeout) { - uint8_t LastACK; - do { - LastACK = suart_getc_(); + uint8_t LastACK = brNONE; + while (!(suart_getc_(&LastACK)) && (Timeout)) { Timeout--; - } while ((suart_timeout) && (Timeout)); - - if(suart_timeout) { - LastACK = brNONE; - } + } ; return (LastACK); } - uint8_t BL_SendCMDKeepAlive(void) { uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0}; @@ -345,19 +310,19 @@ uint8_t BL_PageErase(void) if (BL_SendCMDSetAddress()) { uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01}; BL_SendBuf(sCMD, 2); - return (BL_GetACK((40 / WaitStartBitTimeoutms)) == brSUCCESS); + return (BL_GetACK((40 / START_BIT_TIMEOUT_MS)) == brSUCCESS); } return 0; } uint8_t BL_WriteEEprom(void) { - return BL_WriteA(CMD_PROG_EEPROM, (3000 / WaitStartBitTimeoutms)); + return BL_WriteA(CMD_PROG_EEPROM, (3000 / START_BIT_TIMEOUT_MS)); } uint8_t BL_WriteFlash(void) { - return BL_WriteA(CMD_PROG_FLASH, (40 / WaitStartBitTimeoutms)); + return BL_WriteA(CMD_PROG_FLASH, (40 / START_BIT_TIMEOUT_MS)); } #endif diff --git a/src/main/io/serial_4way_avrootloader.h b/src/main/io/serial_4way_avrootloader.h index 6c4f4d143..2f13e1bb7 100644 --- a/src/main/io/serial_4way_avrootloader.h +++ b/src/main/io/serial_4way_avrootloader.h @@ -24,7 +24,7 @@ void BL_SendBootInit(void); uint8_t BL_ConnectEx(void); -uint8_t BL_SendCMDKeepAlive(void); +uint8_t BL_SendCMDKeepAlive(void); uint8_t BL_PageErase(void); uint8_t BL_ReadEEprom(void); uint8_t BL_WriteEEprom(void); From 5290e4be5637b23b45fac03c45f291c15537b3ac Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 4 Apr 2016 23:28:29 +0200 Subject: [PATCH 31/78] Fix arming on Low Throttle when 3D Mode On switch disabled --- src/main/io/rc_controls.c | 11 ++++-- src/main/io/serial_1wire.h~HEAD | 37 +++++++++++++++++++ ...h~a94ce1fef3a5d63c8af6a85b24f8eba2961e0890 | 36 ++++++++++++++++++ 3 files changed, 80 insertions(+), 4 deletions(-) create mode 100644 src/main/io/serial_1wire.h~HEAD create mode 100644 src/main/io/serial_1wire.h~a94ce1fef3a5d63c8af6a85b24f8eba2961e0890 diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 4b6f8204f..3264b0269 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -113,10 +113,13 @@ bool areSticksInApModePosition(uint16_t ap_mode) throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { - if (feature(FEATURE_3D) && (rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle))) - return THROTTLE_LOW; - else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->mincheck)) - return THROTTLE_LOW; + if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { + if ((rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle))) + return THROTTLE_LOW; + } else { + if (rcData[THROTTLE] < rxConfig->mincheck) + return THROTTLE_LOW; + } return THROTTLE_HIGH; } diff --git a/src/main/io/serial_1wire.h~HEAD b/src/main/io/serial_1wire.h~HEAD new file mode 100644 index 000000000..594cdd43a --- /dev/null +++ b/src/main/io/serial_1wire.h~HEAD @@ -0,0 +1,37 @@ +/* + * 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 . + * + * Ported from https://github.com/4712/BLHeliSuite/blob/master/Interfaces/Arduino1Wire/Source/Arduino1Wire_C/Arduino1Wire.c + * by Nathan Tsoi + */ + +#pragma once + +#ifdef USE_SERIAL_1WIRE + +extern uint8_t escCount; + +typedef struct { + GPIO_TypeDef* gpio; + uint16_t pinpos; + uint16_t pin; +} escHardware_t; + + +void usb1WireInitialize(); +void usb1WirePassthrough(uint8_t escIndex); +void usb1WireDeInitialize(void); +#endif diff --git a/src/main/io/serial_1wire.h~a94ce1fef3a5d63c8af6a85b24f8eba2961e0890 b/src/main/io/serial_1wire.h~a94ce1fef3a5d63c8af6a85b24f8eba2961e0890 new file mode 100644 index 000000000..1e560ed93 --- /dev/null +++ b/src/main/io/serial_1wire.h~a94ce1fef3a5d63c8af6a85b24f8eba2961e0890 @@ -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 . + * + * Ported from https://github.com/4712/BLHeliSuite/blob/master/Interfaces/Arduino1Wire/Source/Arduino1Wire_C/Arduino1Wire.c + * by Nathan Tsoi + */ + +#pragma once + +#ifdef USE_SERIAL_1WIRE + +extern uint8_t escCount; + +typedef struct { + GPIO_TypeDef* gpio; + uint16_t pinpos; + uint16_t pin; +} escHardware_t; + + +void usb1WireInitialize(); +void usb1WirePassthrough(uint8_t escIndex); +#endif From 81b94e5cb80afc8650cebe185adaed988556e5ae Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 5 Apr 2016 00:36:28 +0200 Subject: [PATCH 32/78] Prevent re-arming when gyro calibration on arm enabled (only for switch armers due to safety reasons) --- src/main/mw.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index f795c7b02..11149dc04 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -117,6 +117,7 @@ extern bool antiWindupProtection; uint16_t filteredCycleTime; static bool isRXDataNew; +static bool armingCalibrationWasInitialised; typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype @@ -312,7 +313,7 @@ void annexCode(void) if (ARMING_FLAG(ARMED)) { LED0_ON; } else { - if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { + if (IS_RC_MODE_ACTIVE(BOXARM) == 0 || armingCalibrationWasInitialised) { ENABLE_ARMING_FLAG(OK_TO_ARM); } @@ -341,6 +342,8 @@ void annexCode(void) void mwDisarm(void) { + armingCalibrationWasInitialised = false; + if (ARMING_FLAG(ARMED)) { DISABLE_ARMING_FLAG(ARMED); @@ -366,11 +369,12 @@ void releaseSharedTelemetryPorts(void) { void mwArm(void) { - static bool armingCalibrationWasInitialisedOnce; + static bool firstArmingCalibrationWasCompleted; - if (masterConfig.gyro_cal_on_first_arm && !armingCalibrationWasInitialisedOnce) { + if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) { gyroSetCalibrationCycles(calculateCalibratingCycles()); - armingCalibrationWasInitialisedOnce = true; + armingCalibrationWasInitialised = true; + firstArmingCalibrationWasCompleted = true; } if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated From 8b7b47da9a2b26ac351215ba4b1e81d8f9f01b86 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 6 Apr 2016 00:20:56 +0200 Subject: [PATCH 33/78] Sync Ibus with Cleanflight // Add extra AUX channels --- src/main/rx/ibus.c | 26 ++++++++++++-------------- src/main/rx/ibus.h | 1 + 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index b2028e8ef..299ff4fce 100755 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -13,13 +13,19 @@ * * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . + * + * + * Driver for IBUS (Flysky) receiver + * - initial implementation for MultiWii by Cesco/Plüschi + * - implementation for BaseFlight by Andreas (fiendie) Tacke + * - ported to CleanFlight by Konstantin (digitalentity) Sharlaimov */ #include #include #include -#include "platform.h" +#include #include "build_config.h" @@ -32,9 +38,7 @@ #include "rx/rx.h" #include "rx/ibus.h" -// Driver for IBUS (Flysky) receiver - -#define IBUS_MAX_CHANNEL 8 +#define IBUS_MAX_CHANNEL 10 #define IBUS_BUFFSIZE 32 #define IBUS_SYNCBYTE 0x20 @@ -95,7 +99,7 @@ static void ibusDataReceive(uint16_t c) uint8_t ibusFrameStatus(void) { - uint8_t i; + uint8_t i, offset; uint8_t frameStatus = SERIAL_RX_FRAME_PENDING; uint16_t chksum, rxsum; @@ -112,15 +116,9 @@ uint8_t ibusFrameStatus(void) rxsum = ibus[30] + (ibus[31] << 8); if (chksum == rxsum) { - ibusChannelData[0] = (ibus[ 3] << 8) + ibus[ 2]; - ibusChannelData[1] = (ibus[ 5] << 8) + ibus[ 4]; - ibusChannelData[2] = (ibus[ 7] << 8) + ibus[ 6]; - ibusChannelData[3] = (ibus[ 9] << 8) + ibus[ 8]; - ibusChannelData[4] = (ibus[11] << 8) + ibus[10]; - ibusChannelData[5] = (ibus[13] << 8) + ibus[12]; - ibusChannelData[6] = (ibus[15] << 8) + ibus[14]; - ibusChannelData[7] = (ibus[17] << 8) + ibus[16]; - + for (i = 0, offset = 2; i < IBUS_MAX_CHANNEL; i++, offset += 2) { + ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8); + } frameStatus = SERIAL_RX_FRAME_COMPLETE; } diff --git a/src/main/rx/ibus.h b/src/main/rx/ibus.h index f206052d8..1106659fd 100755 --- a/src/main/rx/ibus.h +++ b/src/main/rx/ibus.h @@ -18,3 +18,4 @@ #pragma once uint8_t ibusFrameStatus(void); +bool ibusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); From 04aaab1cae1f108ed33f530107274141748889b2 Mon Sep 17 00:00:00 2001 From: "Carsten.Wache" Date: Wed, 6 Apr 2016 20:18:33 +0200 Subject: [PATCH 34/78] Send averaged cell voltage on A4 --- src/main/telemetry/smartport.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 281b44dc9..f9060539b 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -83,7 +83,7 @@ enum // remaining 3 bits are crc (according to comments in openTx code) }; -// these data identifiers are obtained from http://diydrones.com/forum/topics/amp-to-frsky-x8r-sport-converter +// these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky.h enum { FSSP_DATAID_SPEED = 0x0830 , @@ -106,6 +106,8 @@ enum FSSP_DATAID_T1 = 0x0400 , FSSP_DATAID_T2 = 0x0410 , FSSP_DATAID_GPS_ALT = 0x0820 , + FSSP_DATAID_A3 = 0x0900 , + FSSP_DATAID_A4 = 0x0910 , }; const uint16_t frSkyDataIdTable[] = { @@ -130,6 +132,7 @@ const uint16_t frSkyDataIdTable[] = { FSSP_DATAID_T1 , FSSP_DATAID_T2 , FSSP_DATAID_GPS_ALT , + FSSP_DATAID_A4 , 0 }; @@ -471,6 +474,12 @@ void handleSmartPortTelemetry(void) } break; #endif + case FSSP_DATAID_A4 : + if (feature(FEATURE_VBAT)) { + smartPortSendPackage(id, vbat * 10 / batteryCellCount ); // given in 0.1V, convert to volts + smartPortHasRequest = 0; + } + break; default: break; // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start From f6b05a489466b2fc17c0d16fbdfd0e53be042483 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 7 Apr 2016 00:39:08 +0200 Subject: [PATCH 35/78] Faster Multishot Math for F1 processors --- src/main/drivers/pwm_output.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index d521153bf..97a8fef65 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -159,7 +159,7 @@ static void pwmWriteOneshot42(uint8_t index, uint16_t value) static void pwmWriteMultiShot(uint8_t index, uint16_t value) { - *motors[index]->ccr = (uint16_t)((float)(value-1000) / 4.1666f)+ 60; + *motors[index]->ccr = 60001 * (value - 1000) / 250000 + 60; } void pwmWriteMotor(uint8_t index, uint16_t value) From d6550827d0156abd6465c6de65998ce4562ce84e Mon Sep 17 00:00:00 2001 From: rigoneri Date: Wed, 6 Apr 2016 22:10:58 -0500 Subject: [PATCH 36/78] Fixed issue that was causing the SPRacingF3Mini target not to build when transponder code was uncommented out. Fixes #179 --- Makefile | 3 ++- src/main/io/serial_cli.c | 2 +- src/main/io/transponder_ir.h | 1 + src/main/main.c | 10 ++++------ 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/Makefile b/Makefile index e4dc3b51a..b5207bdc7 100644 --- a/Makefile +++ b/Makefile @@ -356,7 +356,8 @@ VCP_SRC = \ vcp/usb_istr.c \ vcp/usb_prop.c \ vcp/usb_pwr.c \ - drivers/serial_usb_vcp.c + drivers/serial_usb_vcp.c \ + drivers/usb_io.c NAZE_SRC = startup_stm32f10x_md_gcc.S \ drivers/accgyro_adxl345.c \ diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 1976423cc..3df26829f 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -193,7 +193,7 @@ static const char * const featureNames[] = { "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", - "BLACKBOX", "CHANNEL_FORWARDING", NULL + "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", NULL }; // sync this with rxFailsafeChannelMode_e diff --git a/src/main/io/transponder_ir.h b/src/main/io/transponder_ir.h index 7c52a4cd0..1ccbee77c 100644 --- a/src/main/io/transponder_ir.h +++ b/src/main/io/transponder_ir.h @@ -17,6 +17,7 @@ #pragma once +void transponderInit(uint8_t* transponderCode); void transponderEnable(void); void transponderDisable(void); diff --git a/src/main/main.c b/src/main/main.c index 1450ae07c..125efdd89 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -33,6 +33,7 @@ #include "drivers/sensor.h" #include "drivers/system.h" +#include "drivers/dma.h" #include "drivers/gpio.h" #include "drivers/light_led.h" #include "drivers/sound_beeper.h" @@ -52,9 +53,9 @@ #include "drivers/flash_m25p16.h" #include "drivers/sonar_hcsr04.h" #include "drivers/gyro_sync.h" +#include "drivers/sdcard.h" #include "drivers/usb_io.h" #include "drivers/transponder_ir.h" -#include "drivers/sdcard.h" #include "rx/rx.h" @@ -130,8 +131,6 @@ void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); void spektrumBind(rxConfig_t *rxConfig); const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig); void sonarInit(const sonarHardware_t *sonarHardware); -void transponderInit(uint8_t* transponderCode); -//void usbCableDetectInit(void); #ifdef STM32F303xC // from system_stm32f30x.c @@ -256,6 +255,8 @@ void init(void) timerInit(); // timer must be initialized before any channel is allocated + dmaInit(); + serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); #ifdef USE_SERVOS @@ -534,12 +535,10 @@ void init(void) } #endif -/* TODO - Fix in the future #ifdef USB_CABLE_DETECTION usbCableDetectInit(); #endif - #ifdef TRANSPONDER if (feature(FEATURE_TRANSPONDER)) { transponderInit(masterConfig.transponderData); @@ -548,7 +547,6 @@ void init(void) systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; } #endif -*/ #ifdef USE_FLASHFS #ifdef NAZE From 3cad4793e9e64083ddaae384d87d1bb943b66190 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 8 Apr 2016 11:14:43 +0200 Subject: [PATCH 37/78] SPRACINGF3EVO initial support --- Makefile | 28 +- fake_travis_build.sh | 1 + src/main/drivers/pwm_mapping.c | 61 +++ src/main/drivers/timer.c | 30 ++ src/main/io/serial_msp.c | 2 +- src/main/sensors/initialisation.c | 2 +- .../target/SPRACINGF3EVO/system_stm32f30x.c | 371 ++++++++++++++++++ .../target/SPRACINGF3EVO/system_stm32f30x.h | 76 ++++ src/main/target/SPRACINGF3EVO/target.h | 239 +++++++++++ top_makefile | 2 + 10 files changed, 807 insertions(+), 5 deletions(-) create mode 100755 src/main/target/SPRACINGF3EVO/system_stm32f30x.c create mode 100755 src/main/target/SPRACINGF3EVO/system_stm32f30x.h create mode 100755 src/main/target/SPRACINGF3EVO/target.h diff --git a/Makefile b/Makefile index b5207bdc7..a20beb44a 100644 --- a/Makefile +++ b/Makefile @@ -42,7 +42,7 @@ FORKNAME = betaflight CC3D_TARGETS = CC3D CC3D_OPBL -VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 $(CC3D_TARGETS) CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENFLIGHTF1 ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO IRCFUSIONF3 AFROMINI SPRACINGF3MINI +VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 $(CC3D_TARGETS) CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENFLIGHTF1 ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO IRCFUSIONF3 AFROMINI SPRACINGF3MINI SPRACINGF3EVO # Valid targets for OP VCP support VCP_VALID_TARGETS = $(CC3D_TARGETS) @@ -52,9 +52,9 @@ OPBL_VALID_TARGETS = CC3D_OPBL 64K_TARGETS = CJMCU 128K_TARGETS = ALIENFLIGHTF1 $(CC3D_TARGETS) NAZE OLIMEXINO RMDO AFROMINI -256K_TARGETS = EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB SPRACINGF3MINI +256K_TARGETS = EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB SPRACINGF3MINI SPRACINGF3EVO -F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI +F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI SPRACINGF3EVO # Configure default flash sizes for the targets @@ -710,6 +710,28 @@ IRCFUSIONF3_SRC = \ $(HIGHEND_SRC) \ $(COMMON_SRC) +SPRACINGF3EVO_SRC = \ + $(STM32F30x_COMMON_SRC) \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/compass_ak8963.c \ + drivers/display_ug2864hsweg01.h \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_usb_vcp.c \ + drivers/sdcard.c \ + drivers/sdcard_standard.c \ + drivers/transponder_ir.c \ + drivers/transponder_ir_stm32f30x.c \ + io/asyncfatfs/asyncfatfs.c \ + io/asyncfatfs/fat_standard.c \ + io/transponder_ir.c \ + $(HIGHEND_SRC) \ + $(COMMON_SRC) \ + $(VCP_SRC) + MOTOLAB_SRC = \ $(STM32F30x_COMMON_SRC) \ drivers/accgyro_mpu.c \ diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 9f2d05e67..e7b9082f1 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -6,6 +6,7 @@ targets=("PUBLISHMETA=True" \ "TARGET=COLIBRI_RACE" \ "TARGET=LUX_RACE" \ "TARGET=SPRACINGF3" \ + "TARGET=SPRACINGF3EVO" \ "TARGET=SPRACINGF3MINI" \ "TARGET=NAZE" \ "TARGET=AFROMINI" \ diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 28651dd00..942a68025 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -479,6 +479,67 @@ static const uint16_t airPWM[] = { }; #endif +#ifdef SPRACINGF3EVO +static const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t multiPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t airPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; +#endif + #if defined(MOTOLAB) static const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index cb5c30fcc..87637e322 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -278,6 +278,36 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #endif +#if defined(SPRACINGF3EVO) +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + // PPM / UART2 RX + { TIM8, GPIOA, Pin_15, TIM_Channel_1, TIM8_CC_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource15, GPIO_AF_2}, // PPM + + { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1}, // PWM1 + { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1}, // PWM2 + { TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9}, // PWM3 + { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9}, // PWM4 + { TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2}, // PWM5 + { TIM3, GPIOA, Pin_7, TIM_Channel_2, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_2}, // PWM6 + { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2}, // PWM7 + { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2}, // PWM8 + + // UART3 RX/TX + { TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_1}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) + { TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_1}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) + + // IR / LED Strip Pad + { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6}, // GPIO_TIMER / LED_STRIP +}; + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) + +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3) +#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8 | RCC_APB2Periph_TIM15) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) + +#endif + #if defined(MOTOLAB) const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, GPIOA, Pin_4, TIM_Channel_2, TIM3_IRQn, 1, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2}, // PWM1 - PA4 - *TIM3_CH2 diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index e3ec3579e..85931b023 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -131,7 +131,7 @@ void setGyroSamplingSpeed(uint16_t looptime) { masterConfig.mag_hardware = 1; masterConfig.pid_process_denom = 2; } else if (looptime < 375) { -#if defined(LUX_RACE) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(ALIENFLIGHTF3) +#if defined(LUX_RACE) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(ALIENFLIGHTF3) || defined(SPRACINGF3EVO) masterConfig.acc_hardware = 0; #else masterConfig.acc_hardware = 1; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index cad9b5fe6..5b6c6074f 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -117,7 +117,7 @@ const extiConfig_t *selectMPUIntExtiConfig(void) #endif #endif -#if defined(SPRACINGF3) || defined(SPRACINGF3MINI) +#if defined(SPRACINGF3) || defined(SPRACINGF3MINI) || defined(SPRACINGF3EVO) static const extiConfig_t spRacingF3MPUIntExtiConfig = { .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, .gpioPort = GPIOC, diff --git a/src/main/target/SPRACINGF3EVO/system_stm32f30x.c b/src/main/target/SPRACINGF3EVO/system_stm32f30x.c new file mode 100755 index 000000000..b750c1d9c --- /dev/null +++ b/src/main/target/SPRACINGF3EVO/system_stm32f30x.c @@ -0,0 +1,371 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.c + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. + * This file contains the system clock configuration for STM32F30x devices, + * and is generated by the clock configuration tool + * stm32f30x_Clock_Configuration_V1.0.0.xls + * + * 1. This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): Setups the system clock (System clock source, PLL Multiplier + * and Divider factors, AHB/APBx prescalers and Flash settings), + * depending on the configuration made in the clock xls tool. + * This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32f30x.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must + * be called whenever the core clock is changed + * during program execution. + * + * 2. After each device reset the HSI (8 MHz) is used as system clock source. + * Then SystemInit() function is called, in "startup_stm32f30x.s" file, to + * configure the system clock before to branch to main program. + * + * 3. If the system clock source selected by user fails to startup, the SystemInit() + * function will do nothing and HSI still used as system clock source. User can + * add some code to deal with this issue inside the SetSysClock() function. + * + * 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define + * in "stm32f30x.h" file. When HSE is used as system clock source, directly or + * through PLL, and you are using different crystal you have to adapt the HSE + * value to your own configuration. + * + * 5. This file configures the system clock as follows: + *============================================================================= + * Supported STM32F30x device + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 2 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 2 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 8000000 + *---------------------------------------------------------------------------- + * PLLMUL | 9 + *----------------------------------------------------------------------------- + * PREDIV | 1 + *----------------------------------------------------------------------------- + * USB Clock | ENABLE + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 2 + *----------------------------------------------------------------------------- + * Prefetch Buffer | ON + *----------------------------------------------------------------------------- + *============================================================================= + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** @addtogroup STM32F30x_System_Private_Includes + * @{ + */ + +#include "stm32f30x.h" + +uint32_t hse_value = HSE_VALUE; + +/** + * @} + */ + +/* Private typedef -----------------------------------------------------------*/ + +/** @addtogroup STM32F30x_System_Private_Defines + * @{ + */ +/*!< Uncomment the following line if you need to relocate your vector Table in + Internal SRAM. */ +/* #define VECT_TAB_SRAM */ +#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +/** + * @} + */ + +/* Private macro -------------------------------------------------------------*/ + +/** @addtogroup STM32F30x_System_Private_Variables + * @{ + */ + + uint32_t SystemCoreClock = 72000000; + + __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_FunctionPrototypes + * @{ + */ + +void SetSysClock(void); + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_Functions + * @{ + */ + +/** + * @brief Setup the microcontroller system + * Initialize the Embedded Flash Interface, the PLL and update the + * SystemFrequency variable. + * @param None + * @retval None + */ +void SystemInit(void) +{ + /* FPU settings ------------------------------------------------------------*/ + #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ + #endif + + /* Reset the RCC clock configuration to the default reset state ------------*/ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001; + + /* Reset CFGR register */ + RCC->CFGR &= 0xF87FC00C; + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFF; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; + + /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */ + RCC->CFGR &= (uint32_t)0xFF80FFFF; + + /* Reset PREDIV1[3:0] bits */ + RCC->CFGR2 &= (uint32_t)0xFFFFFFF0; + + /* Reset USARTSW[1:0], I2CSW and TIMs bits */ + RCC->CFGR3 &= (uint32_t)0xFF00FCCC; + + /* Disable all interrupts */ + RCC->CIR = 0x00000000; + + /* Configure the System clock source, PLL Multiplier and Divider factors, + AHB/APBx prescalers and Flash settings ----------------------------------*/ + //SetSysClock(); // called from main() + +#ifdef VECT_TAB_SRAM + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */ +#else + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */ +#endif +} + +/** + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * or HSI_VALUE(*) multiplied/divided by the PLL factors. + * + * (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz), user has to ensure that HSE_VALUE is same as the real + * frequency of the crystal used. Otherwise, this function may + * have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param None + * @retval None + */ +void SystemCoreClockUpdate (void) +{ + uint32_t tmp = 0, pllmull = 0, pllsource = 0, prediv1factor = 0; + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case 0x00: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + case 0x04: /* HSE used as system clock */ + SystemCoreClock = HSE_VALUE; + break; + case 0x08: /* PLL used as system clock */ + /* Get PLL clock source and multiplication factor ----------------------*/ + pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; + pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; + pllmull = ( pllmull >> 18) + 2; + + if (pllsource == 0x00) + { + /* HSI oscillator clock divided by 2 selected as PLL clock entry */ + SystemCoreClock = (HSI_VALUE >> 1) * pllmull; + } + else + { + prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1; + /* HSE oscillator clock selected as PREDIV1 clock entry */ + SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; + } + break; + default: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK clock frequency ----------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; +} + +/** + * @brief Configures the System clock source, PLL Multiplier and Divider factors, + * AHB/APBx prescalers and Flash settings + * @note This function should be called only once the RCC clock configuration + * is reset to the default reset state (done in SystemInit() function). + * @param None + * @retval None + */ +void SetSysClock(void) +{ + __IO uint32_t StartUpCounter = 0, HSEStatus = 0; + +/******************************************************************************/ +/* PLL (clocked by HSE) used as System clock source */ +/******************************************************************************/ + + /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/ + /* Enable HSE */ + RCC->CR |= ((uint32_t)RCC_CR_HSEON); + + /* Wait till HSE is ready and if Time out is reached exit */ + do + { + HSEStatus = RCC->CR & RCC_CR_HSERDY; + StartUpCounter++; + } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + + if ((RCC->CR & RCC_CR_HSERDY) != RESET) + { + HSEStatus = (uint32_t)0x01; + } + else + { + HSEStatus = (uint32_t)0x00; + } + + if (HSEStatus == (uint32_t)0x01) + { + /* Enable Prefetch Buffer and set Flash Latency */ + FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1; + + /* HCLK = SYSCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1; + + /* PCLK2 = HCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1; + + /* PCLK1 = HCLK / 2 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2; + + /* PLL configuration */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL)); + RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9); + + /* Enable PLL */ + RCC->CR |= RCC_CR_PLLON; + + /* Wait till PLL is ready */ + while((RCC->CR & RCC_CR_PLLRDY) == 0) + { + } + + /* Select PLL as system clock source */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); + RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL; + + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL) + { + } + } + else + { /* If HSE fails to start-up, the application will have wrong clock + configuration. User can add here some code to deal with this error */ + } +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/target/SPRACINGF3EVO/system_stm32f30x.h b/src/main/target/SPRACINGF3EVO/system_stm32f30x.h new file mode 100755 index 000000000..b14d1a565 --- /dev/null +++ b/src/main/target/SPRACINGF3EVO/system_stm32f30x.h @@ -0,0 +1,76 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.h + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** + * @brief Define to prevent recursive inclusion + */ +#ifndef __SYSTEM_STM32F30X_H +#define __SYSTEM_STM32F30X_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Exported types ------------------------------------------------------------*/ +extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/** @addtogroup STM32F30x_System_Exported_Functions + * @{ + */ + +extern void SystemInit(void); +extern void SystemCoreClockUpdate(void); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /*__SYSTEM_STM32F30X_H */ + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h new file mode 100755 index 000000000..6e511e3ab --- /dev/null +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -0,0 +1,239 @@ +/* + * 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 "SPEV" + +#define LED0_GPIO GPIOB +#define LED0_PIN Pin_8 +#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB + +#define BEEP_GPIO GPIOC +#define BEEP_PIN Pin_15 +#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC +#define BEEPER_INVERTED + +#define USABLE_TIMER_CHANNEL_COUNT 12 // PPM, 8 PWM, UART3 RX/TX, LED Strip + +#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect + +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + + +#define GYRO +//#define USE_FAKE_GYRO +#define USE_GYRO_SPI_MPU6500 + +#define ACC +//#define USE_FAKE_ACC +#define USE_ACC_SPI_MPU6500 + +#define ACC_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG + +#define BARO +#define USE_BARO_BMP280 + +#define MAG +#define USE_MPU9250_MAG // Enables bypass configuration +#define USE_MAG_AK8963 +//#define USE_MAG_HMC5883 // External + +#define MAG_AK8963_ALIGN CW90_DEG_FLIP + +//#define SONAR +#define BEEPER +#define LED0 + +#define USB_IO + +#define USE_VCP +#define USE_USART1 +#define USE_USART2 +#define USE_USART3 +#define SERIAL_PORT_COUNT 4 + +#ifndef UART1_GPIO +#define UART1_TX_PIN GPIO_Pin_9 // PA9 +#define UART1_RX_PIN GPIO_Pin_10 // PA10 +#define UART1_GPIO GPIOA +#define UART1_GPIO_AF GPIO_AF_7 +#define UART1_TX_PINSOURCE GPIO_PinSource9 +#define UART1_RX_PINSOURCE GPIO_PinSource10 +#endif + +#define UART2_TX_PIN GPIO_Pin_14 // PA14 / SWCLK +#define UART2_RX_PIN GPIO_Pin_15 // PA15 +#define UART2_GPIO GPIOA +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource14 +#define UART2_RX_PINSOURCE GPIO_PinSource15 + +#ifndef UART3_GPIO +#define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7) +#define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7) +#define UART3_GPIO_AF GPIO_AF_7 +#define UART3_GPIO GPIOB +#define UART3_TX_PINSOURCE GPIO_PinSource10 +#define UART3_RX_PINSOURCE GPIO_PinSource11 +#endif + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +#define USE_SPI +#define USE_SPI_DEVICE_1 // PB9,3,4,5 on AF5 SPI1 (MPU) +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 SPI2 (SDCard) + +#define SPI1_GPIO GPIOB +#define SPI1_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB +#define SPI1_NSS_PIN Pin_9 +#define SPI1_NSS_PIN_SOURCE GPIO_PinSource9 +#define SPI1_SCK_PIN Pin_3 +#define SPI1_SCK_PIN_SOURCE GPIO_PinSource3 +#define SPI1_MISO_PIN Pin_4 +#define SPI1_MISO_PIN_SOURCE GPIO_PinSource4 +#define SPI1_MOSI_PIN Pin_5 +#define SPI1_MOSI_PIN_SOURCE GPIO_PinSource5 + +#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_SDCARD +#define USE_SDCARD_SPI2 + +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN GPIO_Pin_14 +#define SDCARD_DETECT_EXTI_LINE EXTI_Line14 +#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource14 +#define SDCARD_DETECT_GPIO_PORT GPIOC +#define SDCARD_DETECT_GPIO_CLK RCC_AHBPeriph_GPIOC +#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOC +#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn + +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_GPIO SPI2_GPIO +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN + +// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +// Note, this is the same DMA channel as USART1_RX. Luckily we don't use DMA for USART Rx. +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 + +#define MPU6500_CS_GPIO_CLK_PERIPHERAL SPI1_GPIO_PERIPHERAL +#define MPU6500_CS_GPIO SPI1_GPIO +#define MPU6500_CS_PIN GPIO_Pin_9 +#define MPU6500_SPI_INSTANCE SPI1 + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER + + +#define ADC_INSTANCE ADC2 +#define ADC_DMA_CHANNEL DMA2_Channel1 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_CHANNEL ADC_Channel_1 + +#define CURRENT_METER_ADC_GPIO GPIOA +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 + +#define RSSI_ADC_GPIO GPIOB +#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_CHANNEL ADC_Channel_12 + +#define LED_STRIP +#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 +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define TRANSPONDER +#define TRANSPONDER_GPIO GPIOA +#define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define TRANSPONDER_GPIO_AF GPIO_AF_6 +#define TRANSPONDER_PIN GPIO_Pin_8 +#define TRANSPONDER_PIN_SOURCE GPIO_PinSource8 +#define TRANSPONDER_TIMER TIM1 +#define TRANSPONDER_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 +#define TRANSPONDER_DMA_CHANNEL DMA1_Channel2 +#define TRANSPONDER_IRQ DMA1_Channel2_IRQn +#define TRANSPONDER_DMA_TC_FLAG DMA1_FLAG_TC2 +#define TRANSPONDER_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM + +#define GPS +#define BLACKBOX +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define TELEMETRY +#define SERIAL_RX +#define DISPLAY +#define USE_SERVOS +#define USE_CLI + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PORT GPIOB +#define BIND_PIN Pin_11 + +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) +#ifdef USE_VCP +#define USE_SERIAL_1WIRE_VCP +#else +#define USE_SERIAL_1WIRE +#endif +#endif + +#define S1W_TX_GPIO UART1_GPIO +#define S1W_TX_PIN UART1_TX_PIN +#define S1W_RX_GPIO UART1_GPIO +#define S1W_RX_PIN UART1_RX_PIN diff --git a/top_makefile b/top_makefile index 71c877f85..e409e50bb 100644 --- a/top_makefile +++ b/top_makefile @@ -2,6 +2,7 @@ ALL_TARGETS := naze ALL_TARGETS += cc3d ALL_TARGETS += cc3d_opbl ALL_TARGETS += spracingf3 +ALL_TARGETS += spracingf3evo ALL_TARGETS += spracingf3mini ALL_TARGETS += sparky ALL_TARGETS += alienflightf1 @@ -20,6 +21,7 @@ clean_cc3d cc3d: opts := TARGET=CC3D clean_cc3d_opbl cc3d_opbl : opts := TARGET=CC3D_OPBL clean_spracingf3mini spracingf3mini : opts := TARGET=SPRACINGF3MINI clean_spracingf3 spracingf3 : opts := TARGET=SPRACINGF3 +clean_spracingf3 spracingf3 : opts := TARGET=SPRACINGF3EVO clean_sparky sparky : opts := TARGET=SPARKY clean_alienflightf1 alienflightf1 : opts := TARGET=ALIENFLIGHTF1 clean_alienflightf3 alienflightf3 : opts := TARGET=ALIENFLIGHTF3 From 50b7f87251099ffe73752713633111ddaad20284 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 8 Apr 2016 14:11:52 +0200 Subject: [PATCH 38/78] Equal PIDs and Rates for rewrite and luxfloat (Luxfloat adopted to rewrite) --- src/main/flight/pid.c | 35 ++++++++++++++++++++--------------- 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9a1e96efa..a2e454f03 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -138,6 +138,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float + // Scaling factors for Pids to match rewrite and use same defaults + static const float luxPTermScale = 1.0f / 128; + static const float luxITermScale = 1000000.0f / 0x1000000; + static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 256; + if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); @@ -145,10 +150,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); // Progressively turn off the horizon self level strength as the stick is banged over horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection - if(pidProfile->H_sensitivity == 0){ + if(pidProfile->D8[PIDLEVEL] == 0){ horizonLevelStrength = 0; } else { - horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1); + horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); } } @@ -158,31 +163,31 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate - AngleRate = (float)((rate + 10) * rcCommand[YAW]) / 50.0f; + AngleRate = (float)((rate + 47) * rcCommand[YAW]) / 32.0f; } else { // ACRO mode, control is GYRO based, direct sticks control is applied to rate PID - AngleRate = (float)((rate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max roll/pitch rate + AngleRate = (float)((rate + 27) * rcCommand[axis]) / 16.0f; // 200dps to 1200dps max roll/pitch rate if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // calculate error angle and limit the angle to the max inclination #ifdef GPS - const float errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here + const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here #else - const float errorAngle = (constrain(rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here + const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here #endif if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed - AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; + AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f; } else { // HORIZON mode - direct sticks control is applied to rate PID // mix up angle error to desired AngleRate to add a little auto-level feel - AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] / 10.0f * horizonLevelStrength; + AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f; } } } - gyroRate = gyroADC[axis] * gyro.scale; // gyro output scaled to dps + gyroRate = gyroADC[axis] / 4.0f; // gyro output scaled to rewrite scale // --------low-level gyro-based PID. ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes @@ -192,9 +197,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate P component if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) { - PTerm = (pidProfile->P8[axis] / 40.0f * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig)); + PTerm = (luxPTermScale * pidProfile->P8[axis] * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig)); } else { - PTerm = RateError * (pidProfile->P8[axis] / 40.0f) * tpaFactor; + PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor; } // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply @@ -203,7 +208,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } // -----calculate I component. - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * getdT() * pidProfile->I8[axis] / 10.0f, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * pidProfile->I8[axis], -250.0f, 250.0f); if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) { if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; @@ -246,7 +251,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // Apply moving average if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]); - DTerm = constrainf(delta * (float)pidProfile->D8[axis] * 0.001f * tpaFactor, -300.0f, 300.0f); + DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f); } // -----calculate total PID output From 216f0d047b9329a1f585d99b0a1724fb20995e5b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 8 Apr 2016 14:20:49 +0200 Subject: [PATCH 39/78] top_makefile fix --- top_makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/top_makefile b/top_makefile index e409e50bb..50385bfa3 100644 --- a/top_makefile +++ b/top_makefile @@ -21,7 +21,7 @@ clean_cc3d cc3d: opts := TARGET=CC3D clean_cc3d_opbl cc3d_opbl : opts := TARGET=CC3D_OPBL clean_spracingf3mini spracingf3mini : opts := TARGET=SPRACINGF3MINI clean_spracingf3 spracingf3 : opts := TARGET=SPRACINGF3 -clean_spracingf3 spracingf3 : opts := TARGET=SPRACINGF3EVO +clean_spracingf3 spracingf3evo : opts := TARGET=SPRACINGF3EVO clean_sparky sparky : opts := TARGET=SPARKY clean_alienflightf1 alienflightf1 : opts := TARGET=ALIENFLIGHTF1 clean_alienflightf3 alienflightf3 : opts := TARGET=ALIENFLIGHTF3 From 5a1cb21ad538876b23d8f06c90b8abb574df8b2e Mon Sep 17 00:00:00 2001 From: 4712 <4712@outlook.de> Date: Fri, 8 Apr 2016 18:47:18 +0200 Subject: [PATCH 40/78] 4way-if cleanup removed superseded 1wire-uart and 1wire-vcp --- Makefile | 2 - src/main/io/serial_1wire.c | 227 ------------------ src/main/io/serial_1wire.h | 37 --- src/main/io/serial_1wire.h~HEAD | 37 --- ...h~a94ce1fef3a5d63c8af6a85b24f8eba2961e0890 | 36 --- src/main/io/serial_1wire_vcp.c | 224 ----------------- src/main/io/serial_1wire_vcp.h | 42 ---- src/main/io/serial_4way.c | 227 ++++++++++++------ src/main/io/serial_4way.h | 75 +++--- src/main/io/serial_4way_avrootloader.c | 87 ++++--- src/main/io/serial_4way_avrootloader.h | 17 +- src/main/io/serial_4way_stk500v2.c | 131 +++++----- src/main/io/serial_4way_stk500v2.h | 14 +- src/main/io/serial_msp.c | 121 +--------- src/main/io/serial_msp.h | 1 - src/main/target/CC3D/target.h | 21 +- src/main/target/COLIBRI_RACE/target.h | 18 +- src/main/target/LUX_RACE/target.h | 19 +- src/main/target/MOTOLAB/target.h | 18 +- src/main/target/NAZE/target.h | 20 +- src/main/target/PORT103R/target.h | 18 +- src/main/target/RMDO/target.h | 18 +- src/main/target/SPARKY/target.h | 18 +- src/main/target/SPRACINGF3/target.h | 18 +- src/main/target/SPRACINGF3EVO/target.h | 16 +- src/main/target/SPRACINGF3MINI/target.h | 18 +- src/main/target/STM32F3DISCOVERY/target.h | 20 +- 27 files changed, 312 insertions(+), 1188 deletions(-) delete mode 100644 src/main/io/serial_1wire.c delete mode 100644 src/main/io/serial_1wire.h delete mode 100644 src/main/io/serial_1wire.h~HEAD delete mode 100644 src/main/io/serial_1wire.h~a94ce1fef3a5d63c8af6a85b24f8eba2961e0890 delete mode 100644 src/main/io/serial_1wire_vcp.c delete mode 100644 src/main/io/serial_1wire_vcp.h diff --git a/Makefile b/Makefile index a20beb44a..a8de88e93 100644 --- a/Makefile +++ b/Makefile @@ -303,8 +303,6 @@ COMMON_SRC = build_config.c \ io/rc_controls.c \ io/rc_curves.c \ io/serial.c \ - io/serial_1wire.c \ - io/serial_1wire_vcp.c \ io/serial_4way.c \ io/serial_4way_avrootloader.c \ io/serial_4way_stk500v2.c \ diff --git a/src/main/io/serial_1wire.c b/src/main/io/serial_1wire.c deleted file mode 100644 index 6b0a5e89a..000000000 --- a/src/main/io/serial_1wire.c +++ /dev/null @@ -1,227 +0,0 @@ -/* - * 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 . - * - * Ported from https://github.com/4712/BLHeliSuite/blob/master/Interfaces/Arduino1Wire/Source/Arduino1Wire_C/Arduino1Wire.c - * by Nathan Tsoi - * Several updates by 4712 in order to optimize interaction with BLHeliSuite - */ - -#include -#include -#include -#include -#include "platform.h" - -#ifdef USE_SERIAL_1WIRE -#include "drivers/gpio.h" -#include "drivers/light_led.h" -#include "drivers/system.h" -#include "io/serial_1wire.h" -#include "io/beeper.h" -#include "drivers/pwm_mapping.h" -#include "drivers/pwm_output.h" -#include "flight/mixer.h" - -uint8_t escCount; // we detect the hardware dynamically - -static escHardware_t escHardware[MAX_PWM_MOTORS]; - -static void gpio_set_mode(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); -} - -static uint32_t GetPinPos(uint32_t pin) { - uint32_t pinPos; - for (pinPos = 0; pinPos < 16; pinPos++) { - uint32_t pinMask = (0x1 << pinPos); - if (pin & pinMask) { - return pinPos; - } - } - return 0; -} - -void usb1WireInitialize() -{ - escCount = 0; - pwmDisableMotors(); - memset(&escHardware,0,sizeof(escHardware)); - pwmOutputConfiguration_t *pwmOutputConfiguration = pwmGetOutputConfiguration(); - for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) { - if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { - if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) { - escHardware[escCount].gpio = pwmOutputConfiguration->portConfigurations[i].timerHardware->gpio; - escHardware[escCount].pin = pwmOutputConfiguration->portConfigurations[i].timerHardware->pin; - escHardware[escCount].pinpos = GetPinPos(escHardware[escCount].pin); - gpio_set_mode(escHardware[escCount].gpio,escHardware[escCount].pin, Mode_IPU); //GPIO_Mode_IPU - escCount++; - } - } - } -} - -void usb1WireDeInitialize(void){ - for (uint8_t selected_esc = 0; selected_esc < (escCount); selected_esc++) { - gpio_set_mode(escHardware[selected_esc].gpio,escHardware[selected_esc].pin, Mode_AF_PP); //GPIO_Mode_IPU - } - escCount = 0; - pwmEnableMotors(); -} - -#ifdef STM32F10X -static volatile uint32_t in_cr_mask, out_cr_mask; - -static __IO uint32_t *cr; -static void gpio_prep_vars(uint32_t escIndex) -{ - GPIO_TypeDef *gpio = escHardware[escIndex].gpio; - uint32_t pinpos = escHardware[escIndex].pinpos; - // mask out extra bits from pinmode, leaving just CNF+MODE - uint32_t inmode = Mode_IPU & 0x0F; - uint32_t outmode = (Mode_Out_PP & 0x0F) | Speed_10MHz; - // reference CRL or CRH, depending whether pin number is 0..7 or 8..15 - cr = &gpio->CRL + (pinpos / 8); - // offset to CNF and MODE portions of CRx register - uint32_t shift = (pinpos % 8) * 4; - // Read out current CRx value - in_cr_mask = out_cr_mask = *cr; - // Mask out 4 bits - in_cr_mask &= ~(0xF << shift); - out_cr_mask &= ~(0xF << shift); - // save current pinmode - in_cr_mask |= inmode << shift; - out_cr_mask |= outmode << shift; -} - -static void gpioSetOne(uint32_t escIndex, GPIO_Mode mode) { - // reference CRL or CRH, depending whether pin number is 0..7 or 8..15 - if (mode == Mode_IPU) { - *cr = in_cr_mask; - escHardware[escIndex].gpio->ODR |= escHardware[escIndex].pin; - } - else { - *cr = out_cr_mask; - } -} -#endif - -#define ESC_HI(escIndex) ((escHardware[escIndex].gpio->IDR & escHardware[escIndex].pin) != (uint32_t)Bit_RESET) -#define RX_HI ((S1W_RX_GPIO->IDR & S1W_RX_PIN) != (uint32_t)Bit_RESET) -#define ESC_SET_HI(escIndex) escHardware[escIndex].gpio->BSRR = escHardware[escIndex].pin -#define ESC_SET_LO(escIndex) escHardware[escIndex].gpio->BRR = escHardware[escIndex].pin -#define TX_SET_HIGH S1W_TX_GPIO->BSRR = S1W_TX_PIN -#define TX_SET_LO S1W_TX_GPIO->BRR = S1W_TX_PIN - -#ifdef STM32F303xC -#define ESC_INPUT(escIndex) escHardware[escIndex].gpio->MODER &= ~(GPIO_MODER_MODER0 << (escHardware[escIndex].pinpos * 2)) -#define ESC_OUTPUT(escIndex) escHardware[escIndex].gpio->MODER |= GPIO_Mode_OUT << (escHardware[escIndex].pinpos * 2) -#endif - -#ifdef STM32F10X -#define ESC_INPUT(escIndex) gpioSetOne(escIndex, Mode_IPU) -#define ESC_OUTPUT(escIndex) gpioSetOne(escIndex, Mode_Out_PP) -#endif - -#define RX_LED_OFF LED0_OFF -#define RX_LED_ON LED0_ON -#define TX_LED_OFF LED1_OFF -#define TX_LED_ON LED1_ON - -// This method translates 2 wires (a tx and rx line) to 1 wire, by letting the -// RX line control when data should be read or written from the single line -void usb1WirePassthrough(uint8_t escIndex) -{ -#ifdef BEEPER - // fix for buzzer often starts beeping continuously when the ESCs are read - // switch beeper silent here - beeperSilence(); -#endif - - // disable all interrupts - __disable_irq(); - - // prepare MSP UART port for direct pin access - // reset all the pins - GPIO_ResetBits(S1W_RX_GPIO, S1W_RX_PIN); - GPIO_ResetBits(S1W_TX_GPIO, S1W_TX_PIN); - // configure gpio - //gpio_set_mode(S1W_RX_GPIO, S1W_RX_PIN, Mode_IPU); - gpio_set_mode(S1W_TX_GPIO, S1W_TX_PIN, Mode_Out_PP); - -#ifdef STM32F10X - // reset our gpio register pointers and bitmask values - gpio_prep_vars(escIndex); -#endif - - ESC_OUTPUT(escIndex); - ESC_SET_HI(escIndex); - TX_SET_HIGH; - // Wait for programmer to go from 1 -> 0 indicating incoming data - while(RX_HI); - - while(1) { - // A new iteration on this loop starts when we have data from the programmer (read_programmer goes low) - // Setup escIndex pin to send data, pullup is the default - ESC_OUTPUT(escIndex); - // Write the first bit - ESC_SET_LO(escIndex); - // Echo on the programmer tx line - TX_SET_LO; - //set LEDs - RX_LED_OFF; - TX_LED_ON; - // Wait for programmer to go 0 -> 1 - uint32_t ct=3333; - while(!RX_HI) { - if (ct > 0) ct--; // count down until 0; - // check for low time ->ct=3333 ~600uS //byte LO time for 0 @ 19200 baud -> 9*52 uS => 468.75uS - // App must send a 0 at 9600 baud (or lower) which has a LO time of at 104uS (or more) > 0 = 937.5uS LO - // BLHeliSuite will use 4800 baud - } - // Programmer is high, end of bit - // At first Echo to the esc, which helps to charge input capacities at ESC - ESC_SET_HI(escIndex); - // Listen to the escIndex, input mode, pullup resistor is on - gpio_set_mode(escHardware[escIndex].gpio, escHardware[escIndex].pin, Mode_IPU); - TX_LED_OFF; - if (ct==0) break; //we reached zero - // Listen to the escIndex while there is no data from the programmer - while (RX_HI) { - if (ESC_HI(escIndex)) { - TX_SET_HIGH; - RX_LED_OFF; - } - else { - TX_SET_LO; - RX_LED_ON; - } - } - } - - // we get here in case ct reached zero - TX_SET_HIGH; - RX_LED_OFF; - // reactivate serial port - gpio_set_mode(S1W_TX_GPIO, S1W_TX_PIN, Mode_AF_PP); - // Enable all irq (for Hardware UART) - __enable_irq(); - return; -} -#endif diff --git a/src/main/io/serial_1wire.h b/src/main/io/serial_1wire.h deleted file mode 100644 index 594cdd43a..000000000 --- a/src/main/io/serial_1wire.h +++ /dev/null @@ -1,37 +0,0 @@ -/* - * 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 . - * - * Ported from https://github.com/4712/BLHeliSuite/blob/master/Interfaces/Arduino1Wire/Source/Arduino1Wire_C/Arduino1Wire.c - * by Nathan Tsoi - */ - -#pragma once - -#ifdef USE_SERIAL_1WIRE - -extern uint8_t escCount; - -typedef struct { - GPIO_TypeDef* gpio; - uint16_t pinpos; - uint16_t pin; -} escHardware_t; - - -void usb1WireInitialize(); -void usb1WirePassthrough(uint8_t escIndex); -void usb1WireDeInitialize(void); -#endif diff --git a/src/main/io/serial_1wire.h~HEAD b/src/main/io/serial_1wire.h~HEAD deleted file mode 100644 index 594cdd43a..000000000 --- a/src/main/io/serial_1wire.h~HEAD +++ /dev/null @@ -1,37 +0,0 @@ -/* - * 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 . - * - * Ported from https://github.com/4712/BLHeliSuite/blob/master/Interfaces/Arduino1Wire/Source/Arduino1Wire_C/Arduino1Wire.c - * by Nathan Tsoi - */ - -#pragma once - -#ifdef USE_SERIAL_1WIRE - -extern uint8_t escCount; - -typedef struct { - GPIO_TypeDef* gpio; - uint16_t pinpos; - uint16_t pin; -} escHardware_t; - - -void usb1WireInitialize(); -void usb1WirePassthrough(uint8_t escIndex); -void usb1WireDeInitialize(void); -#endif diff --git a/src/main/io/serial_1wire.h~a94ce1fef3a5d63c8af6a85b24f8eba2961e0890 b/src/main/io/serial_1wire.h~a94ce1fef3a5d63c8af6a85b24f8eba2961e0890 deleted file mode 100644 index 1e560ed93..000000000 --- a/src/main/io/serial_1wire.h~a94ce1fef3a5d63c8af6a85b24f8eba2961e0890 +++ /dev/null @@ -1,36 +0,0 @@ -/* - * 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 . - * - * Ported from https://github.com/4712/BLHeliSuite/blob/master/Interfaces/Arduino1Wire/Source/Arduino1Wire_C/Arduino1Wire.c - * by Nathan Tsoi - */ - -#pragma once - -#ifdef USE_SERIAL_1WIRE - -extern uint8_t escCount; - -typedef struct { - GPIO_TypeDef* gpio; - uint16_t pinpos; - uint16_t pin; -} escHardware_t; - - -void usb1WireInitialize(); -void usb1WirePassthrough(uint8_t escIndex); -#endif diff --git a/src/main/io/serial_1wire_vcp.c b/src/main/io/serial_1wire_vcp.c deleted file mode 100644 index 6e350f769..000000000 --- a/src/main/io/serial_1wire_vcp.c +++ /dev/null @@ -1,224 +0,0 @@ -/* - * 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 . - * - * Author 4712 - */ - -#include -#include -#include -#include -#include "platform.h" - -#ifdef USE_SERIAL_1WIRE_VCP -#include "drivers/gpio.h" -#include "drivers/light_led.h" -#include "drivers/system.h" -#include "io/serial_1wire_vcp.h" -#include "io/beeper.h" -#include "drivers/pwm_mapping.h" -#include "drivers/pwm_output.h" -#include "flight/mixer.h" -#include "io/serial_msp.h" -#include "drivers/buf_writer.h" -#include "drivers/serial_usb_vcp.h" - -uint8_t escCount; - -static escHardware_t escHardware[MAX_PWM_MOTORS]; - -static uint32_t GetPinPos(uint32_t pin) { - uint32_t pinPos; - for (pinPos = 0; pinPos < 16; pinPos++) { - uint32_t pinMask = (0x1 << pinPos); - if (pin & pinMask) { - return pinPos; - } - } - return 0; -} -static uint8_t selected_esc; - -#define ESC_IS_HI digitalIn(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) != Bit_RESET -#define ESC_IS_LO digitalIn(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) == Bit_RESET -#define ESC_SET_HI digitalHi(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) -#define ESC_SET_LO digitalLo(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) - -#define ESC_INPUT gpioInit(escHardware[selected_esc].gpio, &escHardware[selected_esc].gpio_config_INPUT) -#define ESC_OUTPUT gpioInit(escHardware[selected_esc].gpio, &escHardware[selected_esc].gpio_config_OUTPUT) - -void usb1WireInitializeVcp(void) -{ - pwmDisableMotors(); - selected_esc = 0; - memset(&escHardware, 0, sizeof(escHardware)); - pwmOutputConfiguration_t *pwmOutputConfiguration = pwmGetOutputConfiguration(); - for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) { - if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { - if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) { - escHardware[selected_esc].gpio = pwmOutputConfiguration->portConfigurations[i].timerHardware->gpio; - escHardware[selected_esc].pin = pwmOutputConfiguration->portConfigurations[i].timerHardware->pin; - escHardware[selected_esc].pinpos = GetPinPos(escHardware[selected_esc].pin); - escHardware[selected_esc].gpio_config_INPUT.pin = escHardware[selected_esc].pin; - escHardware[selected_esc].gpio_config_INPUT.speed = Speed_2MHz; // see pwmOutConfig() - escHardware[selected_esc].gpio_config_INPUT.mode = Mode_IPU; - escHardware[selected_esc].gpio_config_OUTPUT = escHardware[selected_esc].gpio_config_INPUT; - escHardware[selected_esc].gpio_config_OUTPUT.mode = Mode_Out_PP; - ESC_INPUT; - ESC_SET_HI; - selected_esc++; - } - } - } - escCount = selected_esc; - selected_esc = 0; -} - -void usb1WireDeInitializeVcp(void){ - for (selected_esc = 0; selected_esc < (escCount); selected_esc++) { - escHardware[selected_esc].gpio_config_OUTPUT.mode = Mode_AF_PP; // see pwmOutConfig() - ESC_OUTPUT; - ESC_SET_LO; - } - escCount = 0; - pwmEnableMotors(); -} - -#define START_BIT_TIMEOUT_MS 2 - -#define BIT_TIME (52) //52uS -#define BIT_TIME_HALVE (BIT_TIME >> 1) //26uS -#define START_BIT_TIME (BIT_TIME_HALVE + 1) -#define STOP_BIT_TIME ((BIT_TIME * 9) + BIT_TIME_HALVE) - -static void suart_putc_(uint8_t *tx_b) -{ - // shift out stopbit first - uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10); - uint32_t btime = micros(); - while(1) { - if(bitmask & 1) { - ESC_SET_HI; // 1 - } else { - ESC_SET_LO; // 0 - } - btime = btime + BIT_TIME; - bitmask = (bitmask >> 1); - if (bitmask == 0) break; // stopbit shifted out - but don't wait - while (micros() < btime); - } -} - -static uint8_t suart_getc_(uint8_t *bt) -{ - uint32_t btime; - uint32_t start_time; - - uint32_t wait_time = millis() + START_BIT_TIMEOUT_MS; - while (ESC_IS_HI) { - // check for startbit begin - if (millis() >= wait_time) { - return 0; - } - } - // start bit - start_time = micros(); - btime = start_time + START_BIT_TIME; - uint16_t bitmask = 0; - uint8_t bit = 0; - while (micros() < btime); - while(1) { - if (ESC_IS_HI) { - bitmask |= (1 << bit); - } - btime = btime + BIT_TIME; - bit++; - if (bit == 10) break; - while (micros() < btime); - } - // check start bit and stop bit - if ((bitmask & 1) || (!(bitmask & (1 << 9)))) { - return 0; - } - *bt = bitmask >> 1; - return 1; -} -#define USE_TXRX_LED - -#ifdef USE_TXRX_LED -#define RX_LED_OFF LED0_OFF -#define RX_LED_ON LED0_ON -#ifdef LED1 -#define TX_LED_OFF LED1_OFF -#define TX_LED_ON LED1_ON -#else -#define TX_LED_OFF LED0_OFF -#define TX_LED_ON LED0_ON -#endif -#else -#define RX_LED_OFF -#define RX_LED_ON -#define TX_LED_OFF -#define TX_LED_ON -#endif - -// This method translates 2 wires (a tx and rx line) to 1 wire, by letting the -// RX line control when data should be read or written from the single line -void usb1WirePassthroughVcp(mspPort_t *mspPort, bufWriter_t *bufwriter, uint8_t escIndex) -{ -#ifdef BEEPER - // fix for buzzer often starts beeping continuously when the ESCs are read - // switch beeper silent here - // TODO (4712) do we need beeperSilence()? - // beeperSilence(); -#endif - - selected_esc = escIndex; - // TODO (4712) check all possible baud rate ok? - // uint32_t baudrate = serialGetBaudRate(mspPort->port); - // serialSetBaudRate(mspPort->port, 19200); - - while(usbVcpGetBaudRate(mspPort->port) != 4800) { - // esc line is in Mode_IPU by default - static uint8_t bt; - - if (suart_getc_(&bt)) { - RX_LED_ON; - serialBeginWrite(mspPort->port); - bufWriterAppend(bufwriter, bt); - while (suart_getc_(&bt)){ - bufWriterAppend(bufwriter, bt); - } - serialEndWrite(mspPort->port); - bufWriterFlush(bufwriter); - RX_LED_OFF; - } - if (serialRxBytesWaiting(mspPort->port)) { - ESC_OUTPUT; - TX_LED_ON; - do { - bt = serialRead(mspPort->port); - suart_putc_(&bt); - } while(serialRxBytesWaiting(mspPort->port)); - ESC_INPUT; - TX_LED_OFF; - } - } - // serialSetBaudRate(mspPort->port, baudrate); - return; -} -#endif - diff --git a/src/main/io/serial_1wire_vcp.h b/src/main/io/serial_1wire_vcp.h deleted file mode 100644 index 0f4a6a3dc..000000000 --- a/src/main/io/serial_1wire_vcp.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * 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 . - * - * Author 4712 - */ -#pragma once - -#include "platform.h" -#ifdef USE_SERIAL_1WIRE_VCP -#include "drivers/serial.h" -#include "drivers/buf_writer.h" -#include "drivers/pwm_mapping.h" -#include "io/serial_msp.h" - -extern uint8_t escCount; - -typedef struct { - GPIO_TypeDef* gpio; - uint16_t pinpos; - uint16_t pin; - gpio_config_t gpio_config_INPUT; - gpio_config_t gpio_config_OUTPUT; -} escHardware_t; - -void usb1WireInitializeVcp(void); -void usb1WireDeInitializeVcp(void); -void usb1WirePassthroughVcp(mspPort_t *mspPort, bufWriter_t *bufwriter, uint8_t escIndex); -#endif - diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 0e7fc889f..5f0dfb466 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -15,20 +15,29 @@ * along with Cleanflight. If not, see . * Author: 4712 */ -#include "platform.h" -#if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) + #include #include #include #include #include -#include "drivers/system.h" + +#include "platform.h" +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE +#include "drivers/serial.h" +#include "drivers/buf_writer.h" +#include "drivers/gpio.h" +#include "drivers/timer.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" +#include "drivers/light_led.h" +#include "drivers/system.h" +#include "drivers/buf_writer.h" #include "flight/mixer.h" #include "io/beeper.h" -#include "io/serial_4way.h" #include "io/serial_msp.h" -#include "drivers/buf_writer.h" +#include "io/serial_msp.h" +#include "io/serial_4way.h" #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #include "io/serial_4way_avrootloader.h" @@ -37,11 +46,30 @@ #include "io/serial_4way_stk500v2.h" #endif +#define USE_TXRX_LED + +#ifdef USE_TXRX_LED +#define RX_LED_OFF LED0_OFF +#define RX_LED_ON LED0_ON +#ifdef LED1 +#define TX_LED_OFF LED1_OFF +#define TX_LED_ON LED1_ON +#else +#define TX_LED_OFF LED0_OFF +#define TX_LED_ON LED0_ON +#endif +#else +#define RX_LED_OFF +#define RX_LED_ON +#define TX_LED_OFF +#define TX_LED_ON +#endif + #define SERIAL_4WAY_INTERFACE_NAME_STR "m4wFCIntf" // *** change to adapt Revision #define SERIAL_4WAY_VER_MAIN 14 #define SERIAL_4WAY_VER_SUB_1 (uint8_t) 4 -#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 03 +#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 04 #define SERIAL_4WAY_PROTOCOL_VER 106 // *** end @@ -50,7 +78,6 @@ #error "beware of SERIAL_4WAY_VER_SUB_1 is uint8_t" #endif - #define SERIAL_4WAY_VERSION (uint16_t) ((SERIAL_4WAY_VER_MAIN * 1000) + (SERIAL_4WAY_VER_SUB_1 * 100) + SERIAL_4WAY_VER_SUB_2) #define SERIAL_4WAY_VERSION_HI (uint8_t) (SERIAL_4WAY_VERSION / 100) @@ -58,6 +85,48 @@ static uint8_t escCount; +escHardware_t escHardware[MAX_PWM_MOTORS]; + +uint8_t selected_esc; + +uint8_32_u DeviceInfo; + +#define DeviceInfoSize 4 + +inline bool isMcuConnected(void) +{ + return (DeviceInfo.bytes[0] > 0); +} + +inline bool isEscHi(uint8_t selEsc) +{ + return (digitalIn(escHardware[selEsc].gpio, escHardware[selEsc].pin) != Bit_RESET); +} +inline bool isEscLo(uint8_t selEsc) +{ + return (digitalIn(escHardware[selEsc].gpio, escHardware[selEsc].pin) == Bit_RESET); +} + +inline void setEscHi(uint8_t selEsc) +{ + digitalHi(escHardware[selEsc].gpio, escHardware[selEsc].pin); +} + +inline void setEscLo(uint8_t selEsc) +{ + digitalLo(escHardware[selEsc].gpio, escHardware[selEsc].pin); +} + +inline void setEscInput(uint8_t selEsc) +{ + gpioInit(escHardware[selEsc].gpio, &escHardware[selEsc].gpio_config_INPUT); +} + +inline void setEscOutput(uint8_t selEsc) +{ + gpioInit(escHardware[selEsc].gpio, &escHardware[selEsc].gpio_config_OUTPUT); +} + static uint32_t GetPinPos(uint32_t pin) { uint32_t pinPos; @@ -74,38 +143,37 @@ uint8_t Initialize4WayInterface(void) { // StopPwmAllMotors(); pwmDisableMotors(); - selected_esc = 0; + escCount = 0; memset(&escHardware, 0, sizeof(escHardware)); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmGetOutputConfiguration(); for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) { if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) { - escHardware[selected_esc].gpio = pwmOutputConfiguration->portConfigurations[i].timerHardware->gpio; - escHardware[selected_esc].pin = pwmOutputConfiguration->portConfigurations[i].timerHardware->pin; - escHardware[selected_esc].pinpos = GetPinPos(escHardware[selected_esc].pin); - escHardware[selected_esc].gpio_config_INPUT.pin = escHardware[selected_esc].pin; - escHardware[selected_esc].gpio_config_INPUT.speed = Speed_2MHz; // see pwmOutConfig() - escHardware[selected_esc].gpio_config_INPUT.mode = Mode_IPU; - escHardware[selected_esc].gpio_config_OUTPUT = escHardware[selected_esc].gpio_config_INPUT; - escHardware[selected_esc].gpio_config_OUTPUT.mode = Mode_Out_PP; - ESC_INPUT; - ESC_SET_HI; - selected_esc++; + escHardware[escCount].gpio = pwmOutputConfiguration->portConfigurations[i].timerHardware->gpio; + escHardware[escCount].pin = pwmOutputConfiguration->portConfigurations[i].timerHardware->pin; + escHardware[escCount].pinpos = GetPinPos(escHardware[escCount].pin); + escHardware[escCount].gpio_config_INPUT.pin = escHardware[escCount].pin; + escHardware[escCount].gpio_config_INPUT.speed = Speed_2MHz; // see pwmOutConfig() + escHardware[escCount].gpio_config_INPUT.mode = Mode_IPU; + escHardware[escCount].gpio_config_OUTPUT = escHardware[escCount].gpio_config_INPUT; + escHardware[escCount].gpio_config_OUTPUT.mode = Mode_Out_PP; + setEscInput(escCount); + setEscHi(escCount); + escCount++; } } } - escCount = selected_esc; return escCount; } void DeInitialize4WayInterface(void) { - for (selected_esc = 0; selected_esc < (escCount); selected_esc++) { - escHardware[selected_esc].gpio_config_OUTPUT.mode = Mode_AF_PP; // see pwmOutConfig() - ESC_OUTPUT; - ESC_SET_LO; + while (escCount > 0) { + escCount--; + escHardware[escCount].gpio_config_OUTPUT.mode = Mode_AF_PP; // see pwmOutConfig() + setEscOutput(escCount); + setEscLo(escCount); } - escCount = 0; pwmEnableMotors(); } @@ -265,25 +333,25 @@ uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) { // * End copyright -#define ATMEL_DEVICE_MATCH ((DeviceInfo.words[0] == 0x9307) || (DeviceInfo.words[0] == 0x930A) || \ - (DeviceInfo.words[0] == 0x930F) || (DeviceInfo.words[0] == 0x940B)) +#define ATMEL_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x9307) || (pDeviceInfo->words[0] == 0x930A) || \ + (pDeviceInfo->words[0] == 0x930F) || (pDeviceInfo->words[0] == 0x940B)) -#define SILABS_DEVICE_MATCH ((DeviceInfo.words[0] == 0xF310)||(DeviceInfo.words[0] ==0xF330) || \ - (DeviceInfo.words[0] == 0xF410) || (DeviceInfo.words[0] == 0xF390) || \ - (DeviceInfo.words[0] == 0xF850) || (DeviceInfo.words[0] == 0xE8B1) || \ - (DeviceInfo.words[0] == 0xE8B2)) +#define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] == 0xF310)||(pDeviceInfo->words[0] ==0xF330) || \ + (pDeviceInfo->words[0] == 0xF410) || (pDeviceInfo->words[0] == 0xF390) || \ + (pDeviceInfo->words[0] == 0xF850) || (pDeviceInfo->words[0] == 0xE8B1) || \ + (pDeviceInfo->words[0] == 0xE8B2)) static uint8_t CurrentInterfaceMode; -static uint8_t Connect(void) +static uint8_t Connect(uint8_32_u *pDeviceInfo) { for (uint8_t I = 0; I < 3; ++I) { #if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) - if (Stk_ConnectEx() && ATMEL_DEVICE_MATCH) { + if (Stk_ConnectEx(pDeviceInfo) && ATMEL_DEVICE_MATCH) { CurrentInterfaceMode = imSK; return 1; } else { - if (BL_ConnectEx()) { + if (BL_ConnectEx(pDeviceInfo)) { if SILABS_DEVICE_MATCH { CurrentInterfaceMode = imSIL_BLB; return 1; @@ -294,7 +362,7 @@ static uint8_t Connect(void) } } #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) - if (BL_ConnectEx()) { + if (BL_ConnectEx(pDeviceInfo)) { if SILABS_DEVICE_MATCH { CurrentInterfaceMode = imSIL_BLB; return 1; @@ -304,7 +372,7 @@ static uint8_t Connect(void) } } #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - if (Stk_ConnectEx()) { + if (Stk_ConnectEx(pDeviceInfo)) { CurrentInterfaceMode = imSK; if ATMEL_DEVICE_MATCH return 1; } @@ -322,7 +390,7 @@ static uint8_t ReadByte(void) { return serialRead(_mspPort->port); } -static union uint8_16u CRC_in; +static uint8_16_u CRC_in; static uint8_t ReadByteCrc(void) { uint8_t b = ReadByte(); CRC_in.word = _crc_xmodem_update(CRC_in.word, b); @@ -332,7 +400,7 @@ static void WriteByte(uint8_t b){ bufWriterAppend(_writer, b); } -static union uint8_16u CRCout; +static uint8_16_u CRCout; static void WriteByteCrc(uint8_t b){ WriteByte(b); CRCout.word = _crc_xmodem_update(CRCout.word, b); @@ -345,11 +413,12 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { uint8_t I_PARAM_LEN; uint8_t CMD; uint8_t ACK_OUT; - union uint8_16u CRC_check; - union uint8_16u Dummy; + uint8_16_u CRC_check; + uint8_16_u Dummy; uint8_t O_PARAM_LEN; uint8_t *O_PARAM; uint8_t *InBuff; + ioMem_t ioMem; _mspPort = mspPort; _writer = bufwriter; @@ -375,8 +444,8 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { O_PARAM = &Dummy.bytes[0]; O_PARAM_LEN = 1; CMD = ReadByteCrc(); - D_FLASH_ADDR_H = ReadByteCrc(); - D_FLASH_ADDR_L = ReadByteCrc(); + ioMem.D_FLASH_ADDR_H = ReadByteCrc(); + ioMem.D_FLASH_ADDR_L = ReadByteCrc(); I_PARAM_LEN = ReadByteCrc(); InBuff = ParamBuf; @@ -400,15 +469,15 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { if (ACK_OUT == ACK_OK) { - // D_FLASH_ADDR_H=Adress_H; - // D_FLASH_ADDR_L=Adress_L; - D_PTR_I = ParamBuf; + // wtf.D_FLASH_ADDR_H=Adress_H; + // wtf.D_FLASH_ADDR_L=Adress_L; + ioMem.D_PTR_I = ParamBuf; switch(CMD) { // ******* Interface related stuff ******* case cmd_InterfaceTestAlive: { - if (IsMcuConnected){ + if (isMcuConnected()){ switch(CurrentInterfaceMode) { #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER @@ -499,7 +568,7 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER case imATM_BLB: { - BL_SendCMDRunRestartBootloader(); + BL_SendCMDRunRestartBootloader(&DeviceInfo); break; } #endif @@ -526,7 +595,7 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { } O_PARAM_LEN = DeviceInfoSize; //4 O_PARAM = (uint8_t *)&DeviceInfo; - if(Connect()) { + if(Connect(&DeviceInfo)) { DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode; } else { SET_DISCONNECTED; @@ -561,9 +630,9 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { { Dummy.bytes[0] = ParamBuf[0]; //Address = Page * 512 - D_FLASH_ADDR_H = (Dummy.bytes[0] << 1); - D_FLASH_ADDR_L = 0; - if (!BL_PageErase()) ACK_OUT = ACK_D_GENERAL_ERROR; + ioMem.D_FLASH_ADDR_H = (Dummy.bytes[0] << 1); + ioMem.D_FLASH_ADDR_L = 0; + if (!BL_PageErase(&ioMem)) ACK_OUT = ACK_D_GENERAL_ERROR; break; } default: @@ -576,11 +645,11 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { //*** Device Memory Read Ops *** case cmd_DeviceRead: { - D_NUM_BYTES = ParamBuf[0]; + ioMem.D_NUM_BYTES = ParamBuf[0]; /* - D_FLASH_ADDR_H=Adress_H; - D_FLASH_ADDR_L=Adress_L; - D_PTR_I = BUF_I; + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; */ switch(CurrentInterfaceMode) { @@ -588,7 +657,7 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { case imSIL_BLB: case imATM_BLB: { - if(!BL_ReadFlash(CurrentInterfaceMode)) + if(!BL_ReadFlash(CurrentInterfaceMode, &ioMem)) { ACK_OUT = ACK_D_GENERAL_ERROR; } @@ -598,7 +667,7 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER case imSK: { - if(!Stk_ReadFlash()) + if(!Stk_ReadFlash(&ioMem)) { ACK_OUT = ACK_D_GENERAL_ERROR; } @@ -610,7 +679,7 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { } if (ACK_OUT == ACK_OK) { - O_PARAM_LEN = D_NUM_BYTES; + O_PARAM_LEN = ioMem.D_NUM_BYTES; O_PARAM = (uint8_t *)&ParamBuf; } break; @@ -618,18 +687,18 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { case cmd_DeviceReadEEprom: { - D_NUM_BYTES = ParamBuf[0]; + ioMem.D_NUM_BYTES = ParamBuf[0]; /* - D_FLASH_ADDR_H = Adress_H; - D_FLASH_ADDR_L = Adress_L; - D_PTR_I = BUF_I; + wtf.D_FLASH_ADDR_H = Adress_H; + wtf.D_FLASH_ADDR_L = Adress_L; + wtf.D_PTR_I = BUF_I; */ switch (CurrentInterfaceMode) { #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER case imATM_BLB: { - if (!BL_ReadEEprom()) + if (!BL_ReadEEprom(&ioMem)) { ACK_OUT = ACK_D_GENERAL_ERROR; } @@ -639,7 +708,7 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER case imSK: { - if (!Stk_ReadEEprom()) + if (!Stk_ReadEEprom(&ioMem)) { ACK_OUT = ACK_D_GENERAL_ERROR; } @@ -651,7 +720,7 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { } if(ACK_OUT == ACK_OK) { - O_PARAM_LEN = D_NUM_BYTES; + O_PARAM_LEN = ioMem.D_NUM_BYTES; O_PARAM = (uint8_t *)&ParamBuf; } break; @@ -660,11 +729,11 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { //*** Device Memory Write Ops *** case cmd_DeviceWrite: { - D_NUM_BYTES = I_PARAM_LEN; + ioMem.D_NUM_BYTES = I_PARAM_LEN; /* - D_FLASH_ADDR_H=Adress_H; - D_FLASH_ADDR_L=Adress_L; - D_PTR_I = BUF_I; + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; */ switch (CurrentInterfaceMode) { @@ -672,7 +741,7 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { case imSIL_BLB: case imATM_BLB: { - if (!BL_WriteFlash()) { + if (!BL_WriteFlash(&ioMem)) { ACK_OUT = ACK_D_GENERAL_ERROR; } break; @@ -681,7 +750,7 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER case imSK: { - if (!Stk_WriteFlash()) + if (!Stk_WriteFlash(&ioMem)) { ACK_OUT = ACK_D_GENERAL_ERROR; } @@ -694,12 +763,12 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { case cmd_DeviceWriteEEprom: { - D_NUM_BYTES = I_PARAM_LEN; + ioMem.D_NUM_BYTES = I_PARAM_LEN; ACK_OUT = ACK_D_GENERAL_ERROR; /* - D_FLASH_ADDR_H=Adress_H; - D_FLASH_ADDR_L=Adress_L; - D_PTR_I = BUF_I; + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; */ switch (CurrentInterfaceMode) { @@ -711,7 +780,7 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { } case imATM_BLB: { - if (BL_WriteEEprom()) + if (BL_WriteEEprom(&ioMem)) { ACK_OUT = ACK_OK; } @@ -721,7 +790,7 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER case imSK: { - if (Stk_WriteEEprom()) + if (Stk_WriteEEprom(&ioMem)) { ACK_OUT = ACK_OK; } @@ -744,8 +813,8 @@ void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) { serialBeginWrite(_mspPort->port); WriteByteCrc(cmd_Remote_Escape); WriteByteCrc(CMD); - WriteByteCrc(D_FLASH_ADDR_H); - WriteByteCrc(D_FLASH_ADDR_L); + WriteByteCrc(ioMem.D_FLASH_ADDR_H); + WriteByteCrc(ioMem.D_FLASH_ADDR_L); WriteByteCrc(O_PARAM_LEN); i=O_PARAM_LEN; diff --git a/src/main/io/serial_4way.h b/src/main/io/serial_4way.h index 853b120a2..1a350e682 100644 --- a/src/main/io/serial_4way.h +++ b/src/main/io/serial_4way.h @@ -16,27 +16,14 @@ * Author: 4712 */ -#include -#if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) -#include "drivers/serial.h" -#include "drivers/buf_writer.h" -#include "drivers/gpio.h" -#include "drivers/timer.h" -#include "drivers/pwm_mapping.h" -#include "drivers/light_led.h" -#include "io/serial_msp.h" - +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER #define imC2 0 #define imSIL_BLB 1 #define imATM_BLB 2 #define imSK 3 -#define RX_LED_OFF LED0_OFF -#define RX_LED_ON LED0_ON -#define TX_LED_OFF LED1_OFF -#define TX_LED_ON LED1_ON - typedef struct { GPIO_TypeDef* gpio; uint16_t pinpos; @@ -45,51 +32,45 @@ typedef struct { gpio_config_t gpio_config_OUTPUT; } escHardware_t; -uint8_t selected_esc; +extern uint8_t selected_esc; -escHardware_t escHardware[MAX_PWM_MOTORS]; +bool isEscHi(uint8_t selEsc); +bool isEscLo(uint8_t selEsc); +void setEscHi(uint8_t selEsc); +void setEscLo(uint8_t selEsc); +void setEscInput(uint8_t selEsc); +void setEscOutput(uint8_t selEsc); -#define ESC_IS_HI digitalIn(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) != Bit_RESET -#define ESC_IS_LO digitalIn(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) == Bit_RESET -#define ESC_SET_HI digitalHi(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) -#define ESC_SET_LO digitalLo(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) +#define ESC_IS_HI isEscHi(selected_esc) +#define ESC_IS_LO isEscLo(selected_esc) +#define ESC_SET_HI setEscHi(selected_esc) +#define ESC_SET_LO setEscLo(selected_esc) +#define ESC_INPUT setEscInput(selected_esc) +#define ESC_OUTPUT setEscOutput(selected_esc) -#define ESC_INPUT gpioInit(escHardware[selected_esc].gpio, &escHardware[selected_esc].gpio_config_INPUT) -#define ESC_OUTPUT gpioInit(escHardware[selected_esc].gpio, &escHardware[selected_esc].gpio_config_OUTPUT) +typedef struct ioMem_s { + uint8_t D_NUM_BYTES; + uint8_t D_FLASH_ADDR_H; + uint8_t D_FLASH_ADDR_L; + uint8_t *D_PTR_I; +} ioMem_t; +extern ioMem_t ioMem; - -void delay_us(uint32_t delay); - -union __attribute__ ((packed)) uint8_16u -{ +typedef union __attribute__ ((packed)) { uint8_t bytes[2]; uint16_t word; -}; +} uint8_16_u; -union __attribute__ ((packed)) uint8_32u -{ +typedef union __attribute__ ((packed)) { uint8_t bytes[4]; uint16_t words[2]; uint32_t dword; -}; +} uint8_32_u; -//----------------------------------------------------------------------------------- -// Global VARIABLES -//----------------------------------------------------------------------------------- -uint8_t D_NUM_BYTES; -uint8_t D_FLASH_ADDR_H; -uint8_t D_FLASH_ADDR_L; -uint8_t *D_PTR_I; - -#define DeviceInfoSize 4 - -union uint8_32u DeviceInfo; - -#define IsMcuConnected (DeviceInfo.bytes[0] > 0) +//extern uint8_32_u DeviceInfo; +bool isMcuConnected(void); uint8_t Initialize4WayInterface(void); void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter); void DeInitialize4WayInterface(void); - -#endif diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 9aca1fb2e..a44048b70 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -22,12 +22,20 @@ #include #include #include -#include +#include "platform.h" -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include "drivers/system.h" -#include "io/serial_4way_avrootloader.h" +#include "drivers/serial.h" +#include "drivers/buf_writer.h" +#include "drivers/pwm_mapping.h" +#include "drivers/gpio.h" +#include "io/serial.h" +#include "io/serial_msp.h" #include "io/serial_4way.h" +#include "io/serial_4way_avrootloader.h" +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + // Bootloader commands // RunCmd @@ -62,7 +70,7 @@ #define BIT_TIME (52) //52uS #define BIT_TIME_HALVE (BIT_TIME >> 1) //26uS -#define START_BIT_TIME (BIT_TIME_HALVE)// + 1) +#define START_BIT_TIME (BIT_TIME_HALVE + 1) //#define STOP_BIT_TIME ((BIT_TIME * 9) + BIT_TIME_HALVE) static uint8_t suart_getc_(uint8_t *bt) @@ -120,8 +128,8 @@ static void suart_putc_(uint8_t *tx_b) } } -static union uint8_16u CRC_16; -static union uint8_16u LastCRC_16; +static uint8_16_u CRC_16; +static uint8_16_u LastCRC_16; static void ByteCrc(uint8_t *bt) { @@ -151,7 +159,7 @@ static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len) len--; } while(len > 0); - if(IsMcuConnected) { + if(isMcuConnected()) { //With CRC read 3 more if(!suart_getc_(&LastCRC_16.bytes[0])) goto timeout; if(!suart_getc_(&LastCRC_16.bytes[1])) goto timeout; @@ -177,14 +185,14 @@ static void BL_SendBuf(uint8_t *pstring, uint8_t len) len--; } while (len > 0); - if (IsMcuConnected) { + if (isMcuConnected()) { suart_putc_(&CRC_16.bytes[0]); suart_putc_(&CRC_16.bytes[1]); } ESC_INPUT; } -uint8_t BL_ConnectEx(void) +uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo) { #define BootMsgLen 4 #define DevSignHi (BootMsgLen) @@ -213,9 +221,9 @@ uint8_t BL_ConnectEx(void) } //only 2 bytes used $1E9307 -> 0x9307 - DeviceInfo.bytes[2] = BootInfo[BootMsgLen - 1]; - DeviceInfo.bytes[1] = BootInfo[DevSignHi]; - DeviceInfo.bytes[0] = BootInfo[DevSignLo]; + pDeviceInfo->bytes[2] = BootInfo[BootMsgLen - 1]; + pDeviceInfo->bytes[1] = BootInfo[DevSignHi]; + pDeviceInfo->bytes[0] = BootInfo[DevSignLo]; return (1); } @@ -238,50 +246,50 @@ uint8_t BL_SendCMDKeepAlive(void) return 1; } -void BL_SendCMDRunRestartBootloader(void) +void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo) { uint8_t sCMD[] = {RestartBootloader, 0}; - DeviceInfo.bytes[0] = 1; + pDeviceInfo->bytes[0] = 1; BL_SendBuf(sCMD, 2); //sends simply 4 x 0x00 (CRC =00) return; } -static uint8_t BL_SendCMDSetAddress(void) //supports only 16 bit Adr +static uint8_t BL_SendCMDSetAddress(ioMem_t *pMem) //supports only 16 bit Adr { // skip if adr == 0xFFFF - if((D_FLASH_ADDR_H == 0xFF) && (D_FLASH_ADDR_L == 0xFF)) return 1; - uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, D_FLASH_ADDR_H, D_FLASH_ADDR_L }; + if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; + uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->D_FLASH_ADDR_H, pMem->D_FLASH_ADDR_L }; BL_SendBuf(sCMD, 4); return (BL_GetACK(2) == brSUCCESS); } -static uint8_t BL_SendCMDSetBuffer(void) +static uint8_t BL_SendCMDSetBuffer(ioMem_t *pMem) { - uint8_t sCMD[] = {CMD_SET_BUFFER, 0, 0, D_NUM_BYTES}; - if (D_NUM_BYTES == 0) { + uint8_t sCMD[] = {CMD_SET_BUFFER, 0, 0, pMem->D_NUM_BYTES}; + if (pMem->D_NUM_BYTES == 0) { // set high byte sCMD[2] = 1; } BL_SendBuf(sCMD, 4); if (BL_GetACK(2) != brNONE) return 0; - BL_SendBuf(D_PTR_I, D_NUM_BYTES); + BL_SendBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES); return (BL_GetACK(40) == brSUCCESS); } -static uint8_t BL_ReadA(uint8_t cmd) +static uint8_t BL_ReadA(uint8_t cmd, ioMem_t *pMem) { - if (BL_SendCMDSetAddress()) { - uint8_t sCMD[] = {cmd, D_NUM_BYTES}; + if (BL_SendCMDSetAddress(pMem)) { + uint8_t sCMD[] = {cmd, pMem->D_NUM_BYTES}; BL_SendBuf(sCMD, 2); - return (BL_ReadBuf(D_PTR_I, D_NUM_BYTES )); + return (BL_ReadBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES )); } return 0; } -static uint8_t BL_WriteA(uint8_t cmd, uint32_t timeout) +static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout) { - if (BL_SendCMDSetAddress()) { - if (!BL_SendCMDSetBuffer()) return 0; + if (BL_SendCMDSetAddress(pMem)) { + if (!BL_SendCMDSetBuffer(pMem)) return 0; uint8_t sCMD[] = {cmd, 0x01}; BL_SendBuf(sCMD, 2); return (BL_GetACK(timeout) == brSUCCESS); @@ -290,24 +298,24 @@ static uint8_t BL_WriteA(uint8_t cmd, uint32_t timeout) } -uint8_t BL_ReadFlash(uint8_t interface_mode) +uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem) { if(interface_mode == imATM_BLB) { - return BL_ReadA(CMD_READ_FLASH_ATM); + return BL_ReadA(CMD_READ_FLASH_ATM, pMem); } else { - return BL_ReadA(CMD_READ_FLASH_SIL); + return BL_ReadA(CMD_READ_FLASH_SIL, pMem); } } -uint8_t BL_ReadEEprom(void) +uint8_t BL_ReadEEprom(ioMem_t *pMem) { - return BL_ReadA(CMD_READ_EEPROM); + return BL_ReadA(CMD_READ_EEPROM, pMem); } -uint8_t BL_PageErase(void) +uint8_t BL_PageErase(ioMem_t *pMem) { - if (BL_SendCMDSetAddress()) { + if (BL_SendCMDSetAddress(pMem)) { uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01}; BL_SendBuf(sCMD, 2); return (BL_GetACK((40 / START_BIT_TIMEOUT_MS)) == brSUCCESS); @@ -315,14 +323,15 @@ uint8_t BL_PageErase(void) return 0; } -uint8_t BL_WriteEEprom(void) +uint8_t BL_WriteEEprom(ioMem_t *pMem) { - return BL_WriteA(CMD_PROG_EEPROM, (3000 / START_BIT_TIMEOUT_MS)); + return BL_WriteA(CMD_PROG_EEPROM, pMem, (3000 / START_BIT_TIMEOUT_MS)); } -uint8_t BL_WriteFlash(void) +uint8_t BL_WriteFlash(ioMem_t *pMem) { - return BL_WriteA(CMD_PROG_FLASH, (40 / START_BIT_TIMEOUT_MS)); + return BL_WriteA(CMD_PROG_FLASH, pMem, (40 / START_BIT_TIMEOUT_MS)); } #endif +#endif diff --git a/src/main/io/serial_4way_avrootloader.h b/src/main/io/serial_4way_avrootloader.h index 2f13e1bb7..39cfaaa3d 100644 --- a/src/main/io/serial_4way_avrootloader.h +++ b/src/main/io/serial_4way_avrootloader.h @@ -20,15 +20,12 @@ #pragma once -#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) - void BL_SendBootInit(void); -uint8_t BL_ConnectEx(void); +uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo); uint8_t BL_SendCMDKeepAlive(void); -uint8_t BL_PageErase(void); -uint8_t BL_ReadEEprom(void); -uint8_t BL_WriteEEprom(void); -uint8_t BL_WriteFlash(void); -uint8_t BL_ReadFlash(uint8_t interface_mode); -void BL_SendCMDRunRestartBootloader(void); -#endif +uint8_t BL_PageErase(ioMem_t *pMem); +uint8_t BL_ReadEEprom(ioMem_t *pMem); +uint8_t BL_WriteEEprom(ioMem_t *pMem); +uint8_t BL_WriteFlash(ioMem_t *pMem); +uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem); +void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo); diff --git a/src/main/io/serial_4way_stk500v2.c b/src/main/io/serial_4way_stk500v2.c index 2bd0c6f7c..f6e21d580 100644 --- a/src/main/io/serial_4way_stk500v2.c +++ b/src/main/io/serial_4way_stk500v2.c @@ -17,16 +17,24 @@ * have a look at https://github.com/sim-/tgy/blob/master/boot.inc * for info about the stk500v2 implementation */ -#include -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER #include #include #include #include + +#include "platform.h" +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE +#include "drivers/gpio.h" +#include "drivers/buf_writer.h" +#include "drivers/pwm_mapping.h" +#include "drivers/serial.h" +#include "config/config.h" +#include "io/serial.h" +#include "io/serial_msp.h" +#include "io/serial_4way.h" #include "io/serial_4way_stk500v2.h" #include "drivers/system.h" -#include "io/serial_4way.h" -#include "config/config.h" +#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER #define BIT_LO_US (32) //32uS #define BIT_HI_US (2*BIT_LO_US) @@ -78,18 +86,14 @@ static uint8_t ckSumOut; static void StkSendByte(uint8_t dat) { ckSumOut ^= dat; - for (uint8_t i = 0; i < 8; i++) - { - if (dat & 0x01) - { + for (uint8_t i = 0; i < 8; i++) { + if (dat & 0x01) { // 1-bits are encoded as 64.0us high, 72.8us low (135.8us total). ESC_SET_HI; delay_us(BIT_HI_US); ESC_SET_LO; delay_us(BIT_HI_US); - } - else - { + } else { // 0-bits are encoded as 27.8us high, 34.5us low, 34.4us high, 37.9 low (134.6us total) ESC_SET_HI; delay_us(BIT_LO_US); @@ -111,7 +115,7 @@ static void StkSendPacketHeader(void) StkSendByte(0xFF); StkSendByte(0xFF); StkSendByte(0x7F); - ckSumOut=0; + ckSumOut = 0; StkSendByte(MESSAGE_START); StkSendByte(++SeqNumber); } @@ -134,15 +138,15 @@ static int8_t ReadBit(void) WaitPinLo; WaitPinHi; LastBitTime = micros() - btimer; - if (LastBitTime <= HiLoTsh) - { + if (LastBitTime <= HiLoTsh) { timeout_timer = timeout_timer + STK_BIT_TIMEOUT; WaitPinLo; WaitPinHi; //lo-bit return 0; + } else { + return 1; } - else return 1; timeout: return -1; } @@ -150,8 +154,7 @@ timeout: static uint8_t ReadByte(uint8_t *bt) { *bt = 0; - for (uint8_t i = 0; i < 8; i++) - { + for (uint8_t i = 0; i < 8; i++) { int8_t bit = ReadBit(); if (bit == -1) goto timeout; if (bit == 1) { @@ -164,8 +167,6 @@ timeout: return 0; } - - static uint8_t StkReadLeader(void) { @@ -175,32 +176,24 @@ static uint8_t StkReadLeader(void) // Wait for the first bit uint32_t waitcycl; //250uS each - if((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) - { + if((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) { waitcycl = STK_WAITCYLCES_EXT; - } - else if(StkCmd == CMD_SIGN_ON) - { + } else if(StkCmd == CMD_SIGN_ON) { waitcycl = STK_WAITCYLCES_START; - } - else - { + } else { waitcycl= STK_WAITCYLCES; } - for ( ; waitcycl >0 ; waitcycl--) - { + for ( ; waitcycl >0 ; waitcycl--) { //check is not timeout if (ReadBit() >- 1) break; } //Skip the first bits - if (waitcycl == 0) - { + if (waitcycl == 0){ goto timeout; } - for (uint8_t i = 0; i < 10; i++) - { + for (uint8_t i = 0; i < 10; i++) { if (ReadBit() == -1) goto timeout; } @@ -209,8 +202,7 @@ static uint8_t StkReadLeader(void) // Read until we get a 0 bit int8_t bit; - do - { + do { bit = ReadBit(); if (bit == -1) goto timeout; } while (bit > 0); @@ -222,7 +214,7 @@ timeout: static uint8_t StkRcvPacket(uint8_t *pstring) { uint8_t bt = 0; - union uint8_16u Len; + uint8_16_u Len; IRQ_OFF; if (!StkReadLeader()) goto Err; @@ -275,11 +267,11 @@ static uint8_t _CMD_SPI_MULTI_EX(volatile uint8_t * ResByte,uint8_t Cmd,uint8_t return 0; } -static uint8_t _CMD_LOAD_ADDRESS(void) +static uint8_t _CMD_LOAD_ADDRESS(ioMem_t *pMem) { // ignore 0xFFFF // assume address is set before and we read or write the immediately following package - if((D_FLASH_ADDR_H == 0xFF) && (D_FLASH_ADDR_L == 0xFF)) return 1; + if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; StkCmd = CMD_LOAD_ADDRESS; StkSendPacketHeader(); StkSendByte(0); // hi byte Msg len @@ -288,16 +280,16 @@ static uint8_t _CMD_LOAD_ADDRESS(void) StkSendByte(CMD_LOAD_ADDRESS); StkSendByte(0); StkSendByte(0); - StkSendByte(D_FLASH_ADDR_H); - StkSendByte(D_FLASH_ADDR_L); + StkSendByte(pMem->D_FLASH_ADDR_H); + StkSendByte(pMem->D_FLASH_ADDR_L); StkSendPacketFooter(); return (StkRcvPacket((void *)StkInBuf)); } -static uint8_t _CMD_READ_MEM_ISP() +static uint8_t _CMD_READ_MEM_ISP(ioMem_t *pMem) { uint8_t LenHi; - if (D_NUM_BYTES>0) { + if (pMem->D_NUM_BYTES>0) { LenHi=0; } else { LenHi=1; @@ -308,23 +300,23 @@ static uint8_t _CMD_READ_MEM_ISP() StkSendByte(TOKEN); StkSendByte(StkCmd); StkSendByte(LenHi); - StkSendByte(D_NUM_BYTES); + StkSendByte(pMem->D_NUM_BYTES); StkSendByte(CmdFlashEepromRead); StkSendPacketFooter(); - return (StkRcvPacket(D_PTR_I)); + return (StkRcvPacket(pMem->D_PTR_I)); } -static uint8_t _CMD_PROGRAM_MEM_ISP(void) +static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem) { - union uint8_16u Len; - uint8_t LenLo=D_NUM_BYTES; + uint8_16_u Len; + uint8_t LenLo = pMem->D_NUM_BYTES; uint8_t LenHi; if (LenLo) { - LenHi=0; - Len.word=LenLo+10; + LenHi = 0; + Len.word = LenLo + 10; } else { - LenHi=1; - Len.word=256+10; + LenHi = 1; + Len.word = 256 + 10; } StkSendPacketHeader(); StkSendByte(Len.bytes[1]); // high byte Msg len @@ -341,8 +333,8 @@ static uint8_t _CMD_PROGRAM_MEM_ISP(void) StkSendByte(0); // poll1 StkSendByte(0); // poll2 do { - StkSendByte(*D_PTR_I); - D_PTR_I++; + StkSendByte(*pMem->D_PTR_I); + pMem->D_PTR_I++; LenLo--; } while (LenLo); StkSendPacketFooter(); @@ -361,11 +353,11 @@ uint8_t Stk_SignOn(void) return (StkRcvPacket((void *) StkInBuf)); } -uint8_t Stk_ConnectEx(void) +uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo) { if (Stk_SignOn()) { - if (_CMD_SPI_MULTI_EX(&DeviceInfo.bytes[1],signature_r,0,1)) { - if (_CMD_SPI_MULTI_EX(&DeviceInfo.bytes[0],signature_r,0,2)) { + if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[1], signature_r,0,1)) { + if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[0], signature_r,0,2)) { return 1; } } @@ -388,43 +380,44 @@ uint8_t Stk_Chip_Erase(void) StkSendByte(0x13); StkSendByte(0x76); StkSendPacketFooter(); - return (StkRcvPacket((void *)StkInBuf)); + return (StkRcvPacket(StkInBuf)); } -uint8_t Stk_ReadFlash(void) +uint8_t Stk_ReadFlash(ioMem_t *pMem) { - if (_CMD_LOAD_ADDRESS()) { + if (_CMD_LOAD_ADDRESS(pMem)) { StkCmd = CMD_READ_FLASH_ISP; - return (_CMD_READ_MEM_ISP()); + return (_CMD_READ_MEM_ISP(pMem)); } return 0; } -uint8_t Stk_ReadEEprom(void) +uint8_t Stk_ReadEEprom(ioMem_t *pMem) { - if (_CMD_LOAD_ADDRESS()) { + if (_CMD_LOAD_ADDRESS(pMem)) { StkCmd = CMD_READ_EEPROM_ISP; - return (_CMD_READ_MEM_ISP()); + return (_CMD_READ_MEM_ISP(pMem)); } return 0; } -uint8_t Stk_WriteFlash(void) +uint8_t Stk_WriteFlash(ioMem_t *pMem) { - if (_CMD_LOAD_ADDRESS()) { + if (_CMD_LOAD_ADDRESS(pMem)) { StkCmd = CMD_PROGRAM_FLASH_ISP; - return (_CMD_PROGRAM_MEM_ISP()); + return (_CMD_PROGRAM_MEM_ISP(pMem)); } return 0; } -uint8_t Stk_WriteEEprom(void) +uint8_t Stk_WriteEEprom(ioMem_t *pMem) { - if (_CMD_LOAD_ADDRESS()) { + if (_CMD_LOAD_ADDRESS(pMem)) { StkCmd = CMD_PROGRAM_EEPROM_ISP; - return (_CMD_PROGRAM_MEM_ISP()); + return (_CMD_PROGRAM_MEM_ISP(pMem)); } return 0; } #endif +#endif diff --git a/src/main/io/serial_4way_stk500v2.h b/src/main/io/serial_4way_stk500v2.h index f24b60ae7..80ca89826 100644 --- a/src/main/io/serial_4way_stk500v2.h +++ b/src/main/io/serial_4way_stk500v2.h @@ -15,17 +15,13 @@ * along with Cleanflight. If not, see . * Author: 4712 */ -#include -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - #pragma once uint8_t Stk_SignOn(void); -uint8_t Stk_ConnectEx(void); -uint8_t Stk_ReadEEprom(void); -uint8_t Stk_WriteEEprom(void); -uint8_t Stk_ReadFlash(void); -uint8_t Stk_WriteFlash(void); +uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo); +uint8_t Stk_ReadEEprom(ioMem_t *pMem); +uint8_t Stk_WriteEEprom(ioMem_t *pMem); +uint8_t Stk_ReadFlash(ioMem_t *pMem); +uint8_t Stk_WriteFlash(ioMem_t *pMem); uint8_t Stk_Chip_Erase(void); -#endif diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 85931b023..65a9a0666 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -92,14 +92,7 @@ #include "serial_msp.h" -#ifdef USE_SERIAL_1WIRE -#include "io/serial_1wire.h" -#endif - -#ifdef USE_SERIAL_1WIRE_VCP -#include "io/serial_1wire_vcp.h" -#endif -#if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include "io/serial_4way.h" #endif @@ -1745,117 +1738,7 @@ static bool processInCommand(void) isRebootScheduled = true; break; -#ifdef USE_SERIAL_1WIRE - case MSP_SET_1WIRE: - // 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 - usb1WireInitialize(); - // reply the count of ESC found - headSerialReply(2); - serialize8(escCount); - serialize8(currentPort->port->identifier); - // 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 - - return true; - } else if (i < escCount) { - // Check for channel number 0..ESC_COUNT-1 - // because we do not come back after calling usb1WirePassthrough - // proceed with a success reply first - headSerialReply(0); - tailSerialReply(); - // flush the transmit buffer - bufWriterFlush(writer); - // wait for all data to send - waitForSerialPortToFinishTransmitting(currentPort->port); - // Start to activate here - // motor 1 => index 0 - - // search currentPort portIndex - /* next lines seems to be unnecessary, because the currentPort always point to the same mspPorts[portIndex] - uint8_t portIndex; - for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { - if (currentPort == &mspPorts[portIndex]) { - break; - } - } - */ - // *mspReleasePortIfAllocated(mspSerialPort); // CloseSerialPort also marks currentPort as UNUSED_PORT - usb1WirePassthrough(i); - // Wait a bit more to let App read the 0 byte and/or switch baudrate - // 2ms will most likely do the job, but give some grace time - delay(10); - // rebuild/refill currentPort structure, does openSerialPort if marked UNUSED_PORT - used ports are skiped - // *mspAllocateSerialPorts(&masterConfig.serialConfig); - /* restore currentPort and mspSerialPort - setCurrentPort(&mspPorts[portIndex]); // not needed same index will be restored - */ - // former used MSP uart is active again - // restore MSP_SET_1WIRE as current command for correct headSerialReply(0) - //currentPort->cmdMSP = MSP_SET_1WIRE; - } else if (i == 0xFE) { - usb1WireDeInitialize(); - } else { - // ESC channel higher than max. allowed - 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 - -#ifdef USE_SERIAL_1WIRE_VCP - case MSP_SET_1WIRE: - // 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 - usb1WireInitializeVcp(); - // reply the count of ESC found - headSerialReply(2); - serialize8(escCount); - serialize8(currentPort->port->identifier); - // 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 - - return true; - } else if (i < escCount) { - // Check for channel number 0..ESC_COUNT-1 - // because we do not come back after calling usb1WirePassthrough - // proceed with a success reply first - headSerialReply(0); - tailSerialReply(); - // flush the transmit buffer - bufWriterFlush(writer); - // wait for all data to send - waitForSerialPortToFinishTransmitting(currentPort->port); - // Start to activate here - // motor 1 => index 0 - usb1WirePassthroughVcp(currentPort, writer, i); - // Wait a bit more to let App switch baudrate - delay(10); - // former used MSP uart is still active - // proceed as usual with MSP commands - // and wait to switch to next channel - // rem: App needs to call MSP_SET_1WIRE(0xFE) to deinitialize Passthrough - } else if (i == 0xFE) { - usb1WireDeInitializeVcp(); - } else { - // ESC channel higher than max. allowed - headSerialError(0); - } - break; -#endif -#if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE case MSP_SET_4WAY_IF: // get channel number // switch all motor lines HI diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index c493e37f1..c1041cf61 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -254,7 +254,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values #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_4WAY_IF 245 //in message Sets 4way interface // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports. diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index c3639e4d9..caf6e7b88 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -124,27 +124,8 @@ #define USE_SERVOS #define USE_CLI -#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#define USE_SERIAL_4WAY_SK_BOOTLOADER +#define USE_SERIAL_4WAY_BLHELI_INTERFACE -#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) -#ifdef USE_VCP -#define USE_SERIAL_1WIRE_VCP -#else -#define USE_SERIAL_1WIRE -#endif -#endif - -#ifdef USE_SERIAL_1WIRE -// FlexPort (pin 21/22, TX/RX respectively): -// Note, FlexPort has 10k pullups on both TX and RX -// JST Pin3 TX - connect to external UART/USB RX -#define S1W_TX_GPIO GPIOB -#define S1W_TX_PIN GPIO_Pin_10 -// JST Pin4 RX - connect to external UART/USB TX -#define S1W_RX_GPIO GPIOB -#define S1W_RX_PIN GPIO_Pin_11 -#endif #undef DISPLAY #undef SONAR diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index c618dd14a..4b9f5c634 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -181,20 +181,4 @@ #define USE_SERVOS #define USE_CLI -#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#define USE_SERIAL_4WAY_SK_BOOTLOADER - -#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) -#ifdef USE_VCP -#define USE_SERIAL_1WIRE_VCP -#else -#define USE_SERIAL_1WIRE -#endif -#endif - -#ifdef USE_SERIAL_1WIRE -#define S1W_TX_GPIO GPIOB -#define S1W_TX_PIN GPIO_Pin_10 -#define S1W_RX_GPIO GPIOB -#define S1W_RX_PIN GPIO_Pin_11 -#endif +#define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 3b19243aa..a3b370df7 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -162,21 +162,4 @@ #define BIND_PORT GPIOC #define BIND_PIN Pin_5 -#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#define USE_SERIAL_4WAY_SK_BOOTLOADER - -#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) -#ifdef USE_VCP -#define USE_SERIAL_1WIRE_VCP -#else -#define USE_SERIAL_1WIRE -#endif -#endif - -#ifdef USE_SERIAL_1WIRE -// Untested -#define S1W_TX_GPIO GPIOB -#define S1W_TX_PIN GPIO_Pin_10 -#define S1W_RX_GPIO GPIOB -#define S1W_RX_PIN GPIO_Pin_11 -#endif +#define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index a7961aa4c..9f168587f 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -188,20 +188,4 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_4 -#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#define USE_SERIAL_4WAY_SK_BOOTLOADER - -#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) -#ifdef USE_VCP -#define USE_SERIAL_1WIRE_VCP -#else -#define USE_SERIAL_1WIRE -#endif -#endif - -#ifdef USE_SERIAL_1WIRE -#define S1W_TX_GPIO GPIOB -#define S1W_TX_PIN GPIO_Pin_6 -#define S1W_RX_GPIO GPIOB -#define S1W_RX_PIN GPIO_Pin_7 -#endif +#define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 239d03c66..944206a98 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -188,25 +188,7 @@ #define BIND_PORT GPIOA #define BIND_PIN Pin_3 -#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#define USE_SERIAL_4WAY_SK_BOOTLOADER - -#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) -#ifdef USE_VCP -#define USE_SERIAL_1WIRE_VCP -#else -#define USE_SERIAL_1WIRE -#endif -#endif - -#ifdef USE_SERIAL_1WIRE -// STM32F103CBT6-LQFP48 Pin30 (PA9) TX - PC3 connects to onboard CP2102 RX -#define S1W_TX_GPIO GPIOA -#define S1W_TX_PIN GPIO_Pin_9 -// STM32F103CBT6-LQFP48 Pin31 (PA10) RX - PC1 to onboard CP2102 TX -#define S1W_RX_GPIO GPIOA -#define S1W_RX_PIN GPIO_Pin_10 -#endif +#define USE_SERIAL_4WAY_BLHELI_INTERFACE // alternative defaults for AlienWii32 F1 target #ifdef ALIENWII32 diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 7ed70db57..4f58065da 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -157,20 +157,4 @@ #define USE_SERVOS #define USE_CLI -#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#define USE_SERIAL_4WAY_SK_BOOTLOADER - -#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) -#ifdef USE_VCP -#define USE_SERIAL_1WIRE_VCP -#else -#define USE_SERIAL_1WIRE -#endif -#endif - -#ifdef USE_SERIAL_1WIRE -#define S1W_TX_GPIO GPIOA -#define S1W_TX_PIN GPIO_Pin_9 -#define S1W_RX_GPIO GPIOA -#define S1W_RX_PIN GPIO_Pin_10 -#endif +#define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index b17efe35e..b9f201ab1 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -154,20 +154,4 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_11 -#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#define USE_SERIAL_4WAY_SK_BOOTLOADER - -#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) -#ifdef USE_VCP -#define USE_SERIAL_1WIRE_VCP -#else -#define USE_SERIAL_1WIRE -#endif -#endif - -#ifdef USE_SERIAL_1WIRE -#define S1W_TX_GPIO GPIOA -#define S1W_TX_PIN GPIO_Pin_9 -#define S1W_RX_GPIO GPIOA -#define S1W_RX_PIN GPIO_Pin_10 -#endif +#define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 7f85e4d73..8ff56c86c 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -163,23 +163,7 @@ #define WS2811_IRQ DMA1_Channel7_IRQn #endif -#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#define USE_SERIAL_4WAY_SK_BOOTLOADER - -#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) -#ifdef USE_VCP -#define USE_SERIAL_1WIRE_VCP -#else -#define USE_SERIAL_1WIRE -#endif -#endif - -#ifdef USE_SERIAL_1WIRE -#define S1W_TX_GPIO GPIOB -#define S1W_TX_PIN GPIO_Pin_6 -#define S1W_RX_GPIO GPIOB -#define S1W_RX_PIN GPIO_Pin_7 -#endif +#define USE_SERIAL_4WAY_BLHELI_INTERFACE #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 29c2d216b..c5a0e5265 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -163,20 +163,4 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_11 -#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#define USE_SERIAL_4WAY_SK_BOOTLOADER - -#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) -#ifdef USE_VCP -#define USE_SERIAL_1WIRE_VCP -#else -#define USE_SERIAL_1WIRE -#endif -#endif - -#ifdef USE_SERIAL_1WIRE -#define S1W_TX_GPIO GPIOA -#define S1W_TX_PIN GPIO_Pin_9 -#define S1W_RX_GPIO GPIOA -#define S1W_RX_PIN GPIO_Pin_10 -#endif +#define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 6e511e3ab..27c094834 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -222,18 +222,4 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_11 -#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#define USE_SERIAL_4WAY_SK_BOOTLOADER - -#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) -#ifdef USE_VCP -#define USE_SERIAL_1WIRE_VCP -#else -#define USE_SERIAL_1WIRE -#endif -#endif - -#define S1W_TX_GPIO UART1_GPIO -#define S1W_TX_PIN UART1_TX_PIN -#define S1W_RX_GPIO UART1_GPIO -#define S1W_RX_PIN UART1_RX_PIN +#define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index e9780b134..5459d4f9a 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -232,20 +232,4 @@ #define BINDPLUG_PORT BUTTON_B_PORT #define BINDPLUG_PIN BUTTON_B_PIN -#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#define USE_SERIAL_4WAY_SK_BOOTLOADER - -#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) -#ifdef USE_VCP -#define USE_SERIAL_1WIRE_VCP -#else -#define USE_SERIAL_1WIRE -#endif -#endif - -#ifdef USE_SERIAL_1WIRE -#define S1W_TX_GPIO UART1_GPIO -#define S1W_TX_PIN UART1_TX_PIN -#define S1W_RX_GPIO UART1_GPIO -#define S1W_RX_PIN UART1_RX_PIN -#endif +#define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index e474ef073..8a812d879 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -162,22 +162,4 @@ #define USE_SERVOS #define USE_CLI -#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#define USE_SERIAL_4WAY_SK_BOOTLOADER - -#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) -#ifdef USE_VCP -#define USE_SERIAL_1WIRE_VCP -#else -#define USE_SERIAL_1WIRE -#endif -#endif - -#ifdef USE_SERIAL_1WIRE -// 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 -#endif +#define USE_SERIAL_4WAY_BLHELI_INTERFACE From 20d222c388ea5ede4e036a68d2484782e3c3fd6c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 8 Apr 2016 19:26:12 +0200 Subject: [PATCH 41/78] Fix uninitialized averageSum --- src/main/common/filter.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 8c266a071..7d26d2ca8 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -98,8 +98,8 @@ float applyBiQuadFilter(float sample, biquad_t *state) } int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]) { - int count; - int32_t averageSum; + int count; + int32_t averageSum = 0; for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1]; averageState[0] = input; @@ -109,8 +109,8 @@ int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageS } float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]) { - int count; - float averageSum; + int count; + float averageSum = 0.0f; for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1]; averageState[0] = input; From 555269c2a3c043ebf10e83013891fd9a3b376a4c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 8 Apr 2016 19:52:59 +0200 Subject: [PATCH 42/78] More accurate deg/sec for iterm reset --- src/main/flight/pid.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a2e454f03..044289ee0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -211,11 +211,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * pidProfile->I8[axis], -250.0f, 250.0f); if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) { - if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; + if (ABS(gyroRate / 4.1f) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; } if (axis == YAW) { - if (ABS(gyroRate) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; + if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; } if (antiWindupProtection || motorLimitReached) { @@ -498,11 +498,11 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) { - if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = 0; + if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = 0; } if (axis == YAW) { - if (ABS(gyroRate / 4) >= pidProfile->yawItermResetRate) errorGyroI[axis] = 0; + if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = 0; } if (antiWindupProtection || motorLimitReached) { From eb5cfe19ac01871e3dff2a9b5cebd296b82f4269 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 8 Apr 2016 16:22:54 +0200 Subject: [PATCH 43/78] SPRacingF3EVO - temporary fix for SPI AK8963. See #2022. --- src/main/drivers/compass_ak8963.c | 177 ++++++++++++++++++++++++++++-- src/main/main.c | 4 + 2 files changed, 174 insertions(+), 7 deletions(-) diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index fc583a18d..c1ee643cb 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -24,6 +24,8 @@ #include "platform.h" +#include "debug.h" + #include "common/axis.h" #include "common/maths.h" @@ -69,6 +71,7 @@ #define READ_FLAG 0x80 #define STATUS1_DATA_READY 0x01 +#define STATUS1_DATA_OVERRUN 0x02 #define STATUS2_DATA_ERROR 0x02 #define STATUS2_MAG_SENSOR_OVERFLOW 0x03 @@ -91,6 +94,14 @@ typedef struct ak8963Configuration_s { ak8963Configuration_t ak8963config; static float magGain[3] = { 1.0f, 1.0f, 1.0f }; +// FIXME pretend we have real MPU9250 support +#ifdef MPU6500_SPI_INSTANCE +#define MPU9250_SPI_INSTANCE +#define verifympu9250WriteRegister mpu6500WriteRegister +#define mpu9250WriteRegister mpu6500WriteRegister +#define mpu9250ReadRegister mpu6500ReadRegister +#endif + #ifdef USE_SPI bool ak8963SPIRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf) { @@ -114,6 +125,77 @@ bool ak8963SPIWrite(uint8_t addr_, uint8_t reg_, uint8_t data) } #endif +#ifdef SPRACINGF3EVO + +typedef struct queuedReadState_s { + bool waiting; + uint8_t len; + uint32_t readStartedAt; // time read was queued in micros. +} queuedReadState_t; + +static queuedReadState_t queuedRead = { false, 0, 0}; + +bool ak8963SPIStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_) +{ + if (queuedRead.waiting) { + return false; + } + + queuedRead.len = len_; + + verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read + verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes + + queuedRead.readStartedAt = micros(); + queuedRead.waiting = true; + + return true; +} + +static uint32_t ak8963SPIQueuedReadTimeRemaining(void) +{ + if (!queuedRead.waiting) { + return 0; + } + + int32_t timeSinceStarted = micros() - queuedRead.readStartedAt; + + int32_t timeRemaining = 8000 - timeSinceStarted; + + if (timeRemaining < 0) { + return 0; + } + + return timeRemaining; +} + +bool ak8963SPICompleteRead(uint8_t *buf) +{ + uint32_t timeRemaining = ak8963SPIQueuedReadTimeRemaining(); + + if (timeRemaining > 0) { + delayMicroseconds(timeRemaining); + } + + queuedRead.waiting = false; + + mpu9250ReadRegister(MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer + return true; +} + +#endif + +#ifdef USE_I2C +bool c_i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data) { + return i2cWrite(addr_, reg_, data); +} + +bool c_i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) { + return i2cRead(addr_, reg_, len, buf); +} +#endif + bool ak8963Detect(mag_t *mag) { bool ack = false; @@ -187,26 +269,106 @@ void ak8963Init() ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS2, 1, &status); // Trigger first measurement +#ifdef SPRACINGF3EVO + ack = ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_CONT1); +#else ack = ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); +#endif } +typedef enum { + CHECK_STATUS = 0, + WAITING_FOR_STATUS, + WAITING_FOR_DATA +} ak8963ReadState_e; + bool ak8963Read(int16_t *magData) { bool ack; - uint8_t status; - uint8_t buf[6]; + uint8_t buf[7]; - ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &status); +#ifdef SPRACINGF3EVO + + // we currently need a different approach on the SPRacingF3EVO which has an MPU9250 connected via SPI. + // we cannot use the ak8963SPIRead() method, it is to slow and blocks for far too long. + + static ak8963ReadState_e state = CHECK_STATUS; + + bool retry = true; + +restart: + switch (state) { + case CHECK_STATUS: + ak8963SPIStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1); + state++; + return false; + + case WAITING_FOR_STATUS: { + uint32_t timeRemaining = ak8963SPIQueuedReadTimeRemaining(); + if (timeRemaining) { + return false; + } + + ack = ak8963SPICompleteRead(&buf[0]); + + uint8_t status = buf[0]; + + if (!ack || ((status & STATUS1_DATA_READY) == 0 && (status & STATUS1_DATA_OVERRUN) == 0)) { + // too early. queue the status read again + state = CHECK_STATUS; + if (retry) { + retry = false; + goto restart; + } + return false; + } + + + // read the 6 bytes of data and the status2 register + ak8963SPIStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7); + + state++; + + return false; + } + + case WAITING_FOR_DATA: { + uint32_t timeRemaining = ak8963SPIQueuedReadTimeRemaining(); + if (timeRemaining) { + return false; + } + + ack = ak8963SPICompleteRead(&buf[0]); + + uint8_t status2 = buf[6]; + if (!ack || (status2 & STATUS2_DATA_ERROR) || (status2 & STATUS2_MAG_SENSOR_OVERFLOW)) { + return false; + } + + magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magGain[X]; + magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magGain[Y]; + magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magGain[Z]; + + state = CHECK_STATUS; + + return true; + } + } + + return false; +#else + ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &buf[0]); + + uint8_t status = buf[0]; if (!ack || (status & STATUS1_DATA_READY) == 0) { return false; } - ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, sizeof(buf), buf); + ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7, &buf[0]); - ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS2, 1, &status); - - if (!ack || (status & STATUS2_DATA_ERROR) || (status & STATUS2_MAG_SENSOR_OVERFLOW)) { + uint8_t status2 = buf[6]; + if (!ack || (status2 & STATUS2_DATA_ERROR) || (status2 & STATUS2_MAG_SENSOR_OVERFLOW)) { return false; } @@ -215,4 +377,5 @@ bool ak8963Read(int16_t *magData) magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magGain[Z]; return ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); // start reading again +#endif } diff --git a/src/main/main.c b/src/main/main.c index 125efdd89..59158b2a5 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -698,6 +698,10 @@ int main(void) { #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); +#ifdef SPRACINGF3EVO + // fixme temporary solution for AK6983 via slave I2C on MPU9250 + rescheduleTask(TASK_COMPASS, 1000000 / 40); +#endif #endif #ifdef BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); From d5532cc139a3e3ec10724e36a701e5934f2d1d86 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 8 Apr 2016 21:20:57 +0200 Subject: [PATCH 44/78] SPRACINGF3EVO config --- src/main/config/config.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/config/config.c b/src/main/config/config.c index 999448218..c00314880 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -593,6 +593,13 @@ static void resetConf(void) featureSet(FEATURE_FAILSAFE); #endif +#ifdef SPRACINGF3EVO + featureSet(FEATURE_TRANSPONDER); + featureSet(FEATURE_RSSI_ADC); + featureSet(FEATURE_CURRENT_METER); + featureSet(FEATURE_TELEMETRY); +#endif + // alternative defaults settings for ALIENFLIGHTF1 and ALIENFLIGHTF3 targets #ifdef ALIENFLIGHT featureSet(FEATURE_RX_SERIAL); From 244c091844fc718125658b082a78bb4e3662177f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 8 Apr 2016 22:34:03 +0200 Subject: [PATCH 45/78] Remove Horizon Sensitivity --- src/main/config/config.c | 2 +- src/main/io/serial_cli.c | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index c00314880..6a7192bf1 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -184,7 +184,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; - pidProfile->H_sensitivity = 75; + pidProfile->H_sensitivity = 75; // TODO - Cleanup during next EEPROM changes #ifdef GTUNE pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3df26829f..7106a99fb 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -739,8 +739,6 @@ const clivalue_t valueTable[] = { { "i_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[YAW], .config.minmax = { 0, 200 } }, { "d_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[YAW], .config.minmax = { 0, 200 } }, - { "sensitivity_horizon", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, .config.minmax = { 0, 250 } }, - { "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], .config.minmax = { 0, 200 } }, { "i_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDALT], .config.minmax = { 0, 200 } }, { "d_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDALT], .config.minmax = { 0, 200 } }, From 9386841abcd0c15229fd87532ce0377db8cd177e Mon Sep 17 00:00:00 2001 From: DTF UHF Date: Fri, 8 Apr 2016 16:35:03 -0400 Subject: [PATCH 46/78] Initial commit of DTFc. Adds support for Invensense ICM-20608-G and BMP280 SPI mode. --- Makefile | 20 +- src/main/config/config.c | 2 +- src/main/drivers/accgyro_mpu6500.h | 1 + src/main/drivers/accgyro_spi_mpu6500.c | 2 +- src/main/drivers/barometer_bmp280.c | 65 ++--- src/main/drivers/barometer_bmp280.h | 39 +++ src/main/drivers/barometer_spi_bmp280.c | 110 +++++++ src/main/drivers/barometer_spi_bmp280.h | 24 ++ src/main/drivers/pwm_mapping.c | 54 ++++ src/main/drivers/timer.c | 24 ++ src/main/sensors/initialisation.c | 13 + src/main/target/DOGE/system_stm32f30x.c | 372 ++++++++++++++++++++++++ src/main/target/DOGE/system_stm32f30x.h | 76 +++++ src/main/target/DOGE/target.h | 206 +++++++++++++ 14 files changed, 962 insertions(+), 46 deletions(-) create mode 100644 src/main/drivers/barometer_spi_bmp280.c create mode 100644 src/main/drivers/barometer_spi_bmp280.h create mode 100644 src/main/target/DOGE/system_stm32f30x.c create mode 100644 src/main/target/DOGE/system_stm32f30x.h create mode 100644 src/main/target/DOGE/target.h diff --git a/Makefile b/Makefile index a8de88e93..9bbf85ea5 100644 --- a/Makefile +++ b/Makefile @@ -42,7 +42,7 @@ FORKNAME = betaflight CC3D_TARGETS = CC3D CC3D_OPBL -VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 $(CC3D_TARGETS) CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENFLIGHTF1 ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO IRCFUSIONF3 AFROMINI SPRACINGF3MINI SPRACINGF3EVO +VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 $(CC3D_TARGETS) CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENFLIGHTF1 ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO IRCFUSIONF3 AFROMINI SPRACINGF3MINI SPRACINGF3EVO DOGE # Valid targets for OP VCP support VCP_VALID_TARGETS = $(CC3D_TARGETS) @@ -52,9 +52,9 @@ OPBL_VALID_TARGETS = CC3D_OPBL 64K_TARGETS = CJMCU 128K_TARGETS = ALIENFLIGHTF1 $(CC3D_TARGETS) NAZE OLIMEXINO RMDO AFROMINI -256K_TARGETS = EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB SPRACINGF3MINI SPRACINGF3EVO +256K_TARGETS = EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB SPRACINGF3MINI SPRACINGF3EVO DOGE -F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI SPRACINGF3EVO +F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI SPRACINGF3EVO DOGE # Configure default flash sizes for the targets @@ -624,6 +624,20 @@ LUX_RACE_SRC = \ $(HIGHEND_SRC) \ $(COMMON_SRC) \ $(VCP_SRC) + +DOGE_SRC = \ + $(STM32F30x_COMMON_SRC) \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_spi_bmp280.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_usb_vcp.c \ + $(HIGHEND_SRC) \ + $(COMMON_SRC) \ + $(VCP_SRC) SPARKY_SRC = \ $(STM32F30x_COMMON_SRC) \ diff --git a/src/main/config/config.c b/src/main/config/config.c index c00314880..f9058e4e7 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -380,7 +380,7 @@ static void resetConf(void) masterConfig.version = EEPROM_CONF_VERSION; masterConfig.mixerMode = MIXER_QUADX; featureClearAll(); -#if defined(CJMCU) || defined(SPARKY) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(SPRACINGF3MINI) || defined(LUX_RACE) +#if defined(CJMCU) || defined(SPARKY) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(SPRACINGF3MINI) || defined(LUX_RACE) || defined(DOGE) featureSet(FEATURE_RX_PPM); #endif diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h index 92bbef426..3c2339e32 100644 --- a/src/main/drivers/accgyro_mpu6500.h +++ b/src/main/drivers/accgyro_mpu6500.h @@ -17,6 +17,7 @@ #define MPU6500_WHO_AM_I_CONST (0x70) #define MPU9250_WHO_AM_I_CONST (0x71) +#define ICM20608G_WHO_AM_I_CONST (0xAF) #define MPU6500_BIT_RESET (0x80) diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 0249c2160..5b71f1725 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -107,7 +107,7 @@ bool mpu6500SpiDetect(void) mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp); - if (tmp == MPU6500_WHO_AM_I_CONST || tmp == MPU9250_WHO_AM_I_CONST) { + if (tmp == MPU6500_WHO_AM_I_CONST || tmp == MPU9250_WHO_AM_I_CONST || tmp == ICM20608G_WHO_AM_I_CONST) { return true; } diff --git a/src/main/drivers/barometer_bmp280.c b/src/main/drivers/barometer_bmp280.c index 43de7560e..257f2dd7e 100644 --- a/src/main/drivers/barometer_bmp280.c +++ b/src/main/drivers/barometer_bmp280.c @@ -28,50 +28,12 @@ #include "bus_i2c.h" #include "barometer_bmp280.h" +#include "barometer_spi_bmp280.h" #ifdef BARO // BMP280, address 0x76 -#define BMP280_I2C_ADDR (0x76) -#define BMP280_DEFAULT_CHIP_ID (0x58) - -#define BMP280_CHIP_ID_REG (0xD0) /* Chip ID Register */ -#define BMP280_RST_REG (0xE0) /* Softreset Register */ -#define BMP280_STAT_REG (0xF3) /* Status Register */ -#define BMP280_CTRL_MEAS_REG (0xF4) /* Ctrl Measure Register */ -#define BMP280_CONFIG_REG (0xF5) /* Configuration Register */ -#define BMP280_PRESSURE_MSB_REG (0xF7) /* Pressure MSB Register */ -#define BMP280_PRESSURE_LSB_REG (0xF8) /* Pressure LSB Register */ -#define BMP280_PRESSURE_XLSB_REG (0xF9) /* Pressure XLSB Register */ -#define BMP280_TEMPERATURE_MSB_REG (0xFA) /* Temperature MSB Reg */ -#define BMP280_TEMPERATURE_LSB_REG (0xFB) /* Temperature LSB Reg */ -#define BMP280_TEMPERATURE_XLSB_REG (0xFC) /* Temperature XLSB Reg */ -#define BMP280_FORCED_MODE (0x01) - -#define BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG (0x88) -#define BMP280_PRESSURE_TEMPERATURE_CALIB_DATA_LENGTH (24) -#define BMP280_DATA_FRAME_SIZE (6) - -#define BMP280_OVERSAMP_SKIPPED (0x00) -#define BMP280_OVERSAMP_1X (0x01) -#define BMP280_OVERSAMP_2X (0x02) -#define BMP280_OVERSAMP_4X (0x03) -#define BMP280_OVERSAMP_8X (0x04) -#define BMP280_OVERSAMP_16X (0x05) - -// configure pressure and temperature oversampling, forced sampling mode -#define BMP280_PRESSURE_OSR (BMP280_OVERSAMP_8X) -#define BMP280_TEMPERATURE_OSR (BMP280_OVERSAMP_1X) -#define BMP280_MODE (BMP280_PRESSURE_OSR << 2 | BMP280_TEMPERATURE_OSR << 5 | BMP280_FORCED_MODE) - -#define T_INIT_MAX (20) -// 20/16 = 1.25 ms -#define T_MEASURE_PER_OSRS_MAX (37) -// 37/16 = 2.3125 ms -#define T_SETUP_PRESSURE_MAX (10) -// 10/16 = 0.625 ms - typedef struct bmp280_calib_param_s { uint16_t dig_T1; /* calibration T1 data */ int16_t dig_T2; /* calibration T2 data */ @@ -92,13 +54,15 @@ static uint8_t bmp280_chip_id = 0; static bool bmp280InitDone = false; STATIC_UNIT_TESTED bmp280_calib_param_t bmp280_cal; // uncompensated pressure and temperature -STATIC_UNIT_TESTED int32_t bmp280_up = 0; -STATIC_UNIT_TESTED int32_t bmp280_ut = 0; +int32_t bmp280_up = 0; +int32_t bmp280_ut = 0; static void bmp280_start_ut(void); static void bmp280_get_ut(void); +#ifndef USE_BARO_SPI_BMP280 static void bmp280_start_up(void); static void bmp280_get_up(void); +#endif STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature); bool bmp280Detect(baro_t *baro) @@ -108,6 +72,17 @@ bool bmp280Detect(baro_t *baro) delay(20); +#ifdef USE_BARO_SPI_BMP280 + bmp280SpiInit(); + bmp280ReadRegister(BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); + if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID) + return false; + + // read calibration + bmp280ReadRegister(BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal); + // set oversampling + power mode (forced), and start sampling + bmp280WriteRegister(BMP280_CTRL_MEAS_REG, BMP280_MODE); +#else i2cRead(BMP280_I2C_ADDR, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); /* read Chip Id */ if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID) return false; @@ -116,6 +91,7 @@ bool bmp280Detect(baro_t *baro) i2cRead(BMP280_I2C_ADDR, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal); // set oversampling + power mode (forced), and start sampling i2cWrite(BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE); +#endif bmp280InitDone = true; @@ -126,8 +102,13 @@ bool bmp280Detect(baro_t *baro) // only _up part is executed, and gets both temperature and pressure baro->up_delay = ((T_INIT_MAX + T_MEASURE_PER_OSRS_MAX * (((1 << BMP280_TEMPERATURE_OSR) >> 1) + ((1 << BMP280_PRESSURE_OSR) >> 1)) + (BMP280_PRESSURE_OSR ? T_SETUP_PRESSURE_MAX : 0) + 15) / 16) * 1000; +#ifdef USE_BARO_SPI_BMP280 + baro->start_up = bmp280_spi_start_up; + baro->get_up = bmp280_spi_get_up; +#else baro->start_up = bmp280_start_up; baro->get_up = bmp280_get_up; +#endif baro->calculate = bmp280_calculate; return true; @@ -143,6 +124,7 @@ static void bmp280_get_ut(void) // dummy } +#ifndef USE_BARO_SPI_BMP280 static void bmp280_start_up(void) { // start measurement @@ -159,6 +141,7 @@ static void bmp280_get_up(void) bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4)); bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4)); } +#endif // Returns temperature in DegC, resolution is 0.01 DegC. Output value of "5123" equals 51.23 DegC // t_fine carries fine temperature as global value diff --git a/src/main/drivers/barometer_bmp280.h b/src/main/drivers/barometer_bmp280.h index e6858ed10..b0a4829fb 100644 --- a/src/main/drivers/barometer_bmp280.h +++ b/src/main/drivers/barometer_bmp280.h @@ -17,5 +17,44 @@ #pragma once +#define BMP280_I2C_ADDR (0x76) +#define BMP280_DEFAULT_CHIP_ID (0x58) + +#define BMP280_CHIP_ID_REG (0xD0) /* Chip ID Register */ +#define BMP280_RST_REG (0xE0) /* Softreset Register */ +#define BMP280_STAT_REG (0xF3) /* Status Register */ +#define BMP280_CTRL_MEAS_REG (0xF4) /* Ctrl Measure Register */ +#define BMP280_CONFIG_REG (0xF5) /* Configuration Register */ +#define BMP280_PRESSURE_MSB_REG (0xF7) /* Pressure MSB Register */ +#define BMP280_PRESSURE_LSB_REG (0xF8) /* Pressure LSB Register */ +#define BMP280_PRESSURE_XLSB_REG (0xF9) /* Pressure XLSB Register */ +#define BMP280_TEMPERATURE_MSB_REG (0xFA) /* Temperature MSB Reg */ +#define BMP280_TEMPERATURE_LSB_REG (0xFB) /* Temperature LSB Reg */ +#define BMP280_TEMPERATURE_XLSB_REG (0xFC) /* Temperature XLSB Reg */ +#define BMP280_FORCED_MODE (0x01) + +#define BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG (0x88) +#define BMP280_PRESSURE_TEMPERATURE_CALIB_DATA_LENGTH (24) +#define BMP280_DATA_FRAME_SIZE (6) + +#define BMP280_OVERSAMP_SKIPPED (0x00) +#define BMP280_OVERSAMP_1X (0x01) +#define BMP280_OVERSAMP_2X (0x02) +#define BMP280_OVERSAMP_4X (0x03) +#define BMP280_OVERSAMP_8X (0x04) +#define BMP280_OVERSAMP_16X (0x05) + +// configure pressure and temperature oversampling, forced sampling mode +#define BMP280_PRESSURE_OSR (BMP280_OVERSAMP_8X) +#define BMP280_TEMPERATURE_OSR (BMP280_OVERSAMP_1X) +#define BMP280_MODE (BMP280_PRESSURE_OSR << 2 | BMP280_TEMPERATURE_OSR << 5 | BMP280_FORCED_MODE) + +#define T_INIT_MAX (20) +// 20/16 = 1.25 ms +#define T_MEASURE_PER_OSRS_MAX (37) +// 37/16 = 2.3125 ms +#define T_SETUP_PRESSURE_MAX (10) +// 10/16 = 0.625 ms + bool bmp280Detect(baro_t *baro); diff --git a/src/main/drivers/barometer_spi_bmp280.c b/src/main/drivers/barometer_spi_bmp280.c new file mode 100644 index 000000000..0e047383a --- /dev/null +++ b/src/main/drivers/barometer_spi_bmp280.c @@ -0,0 +1,110 @@ +/* + * This file is part of Betaflight. + * + * Betaflight 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. + * + * Betaflight 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 Betaflight. If not, see . + */ + +#include +#include +#include + +#include + +#include "build_config.h" + +#include "bus_spi.h" + +#include "barometer.h" +#include "barometer_bmp280.h" + +#define DISABLE_BMP280 GPIO_SetBits(BMP280_CS_GPIO, BMP280_CS_PIN) +#define ENABLE_BMP280 GPIO_ResetBits(BMP280_CS_GPIO, BMP280_CS_PIN) + +extern int32_t bmp280_up; +extern int32_t bmp280_ut; + +bool bmp280WriteRegister(uint8_t reg, uint8_t data) +{ + ENABLE_BMP280; + spiTransferByte(BMP280_SPI_INSTANCE, reg & 0x7F); + spiTransferByte(BMP280_SPI_INSTANCE, data); + DISABLE_BMP280; + + return true; +} + +bool bmp280ReadRegister(uint8_t reg, uint8_t length, uint8_t *data) +{ + ENABLE_BMP280; + spiTransferByte(BMP280_SPI_INSTANCE, reg | 0x80); // read transaction + spiTransfer(BMP280_SPI_INSTANCE, data, NULL, length); + DISABLE_BMP280; + + return true; +} + +void bmp280SpiInit(void) +{ + static bool hardwareInitialised = false; + + if (hardwareInitialised) { + return; + } + +#ifdef STM32F303 + RCC_AHBPeriphClockCmd(BMP280_CS_GPIO_CLK_PERIPHERAL, ENABLE); + + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Pin = BMP280_CS_PIN; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + + GPIO_Init(BMP280_CS_GPIO, &GPIO_InitStructure); +#endif + +#ifdef STM32F10X + RCC_APB2PeriphClockCmd(BMP280_CS_GPIO_CLK_PERIPHERAL, ENABLE); + + gpio_config_t gpio; + gpio.mode = Mode_Out_PP; + gpio.pin = BMP280_CS_PIN; + gpio.speed = Speed_50MHz; + gpioInit(BMP280_CS_GPIO, &gpio); +#endif + + GPIO_SetBits(BMP280_CS_GPIO, BMP280_CS_PIN); + + spiSetDivisor(BMP280_SPI_INSTANCE, SPI_9MHZ_CLOCK_DIVIDER); + + hardwareInitialised = true; +} + +void bmp280_spi_start_up(void) +{ + // start measurement + // set oversampling + power mode (forced), and start sampling + bmp280WriteRegister(BMP280_CTRL_MEAS_REG, BMP280_MODE); +} + +void bmp280_spi_get_up(void) +{ + uint8_t data[BMP280_DATA_FRAME_SIZE]; + + // read data from sensor + bmp280ReadRegister(BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data); + bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4)); + bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4)); +} diff --git a/src/main/drivers/barometer_spi_bmp280.h b/src/main/drivers/barometer_spi_bmp280.h new file mode 100644 index 000000000..862c3ad41 --- /dev/null +++ b/src/main/drivers/barometer_spi_bmp280.h @@ -0,0 +1,24 @@ +/* + * This file is part of Betaflight. + * + * Betaflight 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. + * + * Betaflight 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 Betaflight. If not, see . + */ + +#pragma once + +void bmp280SpiInit(void); +bool bmp280ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); +bool bmp280WriteRegister(uint8_t reg, uint8_t data); +void bmp280_spi_start_up(void); +void bmp280_spi_get_up(void); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 942a68025..9c5386327 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -639,6 +639,60 @@ static const uint16_t airPWM[] = { }; #endif +#ifdef DOGE +static const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t multiPWM[] = { + // prevent crashing, but do nothing + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t airPWM[] = { + // prevent crashing, but do nothing + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; +#endif + static const uint16_t * const hardwareMaps[] = { multiPWM, multiPPM, diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 87637e322..f0a8ea960 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -141,6 +141,30 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #endif +#ifdef DOGE +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 0, Mode_AF_PP_PD, GPIO_PinSource8, GPIO_AF_6}, // PWM1 - PA8 + + { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2}, // PWM2 - PB8 + { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2}, // PWM3 - PB9 + + { TIM2, GPIOA, Pin_10, TIM_Channel_4, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_10}, // PMW4 - PA10 + { TIM2, GPIOA, Pin_9, TIM_Channel_3, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_10}, // PWM5 - PA9 + { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1}, // PWM6 - PA0 + { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1}, // PWM7 - PA1 + + { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource0, GPIO_AF_2}, // PWM8 - PB1 + { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 1, Mode_AF_PP_PD, GPIO_PinSource1, GPIO_AF_2}, // PWM9 - PB0 +}; + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) + +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) +#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) + +#endif + #ifdef CHEBUZZF3 const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // INPUTS CH1-8 diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 5b6c6074f..9c49090d4 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -156,6 +156,19 @@ const extiConfig_t *selectMPUIntExtiConfig(void) return &RaceMPUIntExtiConfig; #endif +#if defined(DOGE) + static const extiConfig_t dogeMPUIntExtiConfig = { + .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, + .gpioPort = GPIOC, + .gpioPin = Pin_13, + .exti_port_source = EXTI_PortSourceGPIOC, + .exti_pin_source = EXTI_PinSource13, + .exti_line = EXTI_Line13, + .exti_irqn = EXTI15_10_IRQn + }; + return &dogeMPUIntExtiConfig; +#endif + #if defined(MOTOLAB) || defined(SPARKY) static const extiConfig_t MotolabF3MPU6050Config = { .gpioAHBPeripherals = RCC_AHBPeriph_GPIOA, diff --git a/src/main/target/DOGE/system_stm32f30x.c b/src/main/target/DOGE/system_stm32f30x.c new file mode 100644 index 000000000..fca696982 --- /dev/null +++ b/src/main/target/DOGE/system_stm32f30x.c @@ -0,0 +1,372 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.c + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. + * This file contains the system clock configuration for STM32F30x devices, + * and is generated by the clock configuration tool + * stm32f30x_Clock_Configuration_V1.0.0.xls + * + * 1. This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): Setups the system clock (System clock source, PLL Multiplier + * and Divider factors, AHB/APBx prescalers and Flash settings), + * depending on the configuration made in the clock xls tool. + * This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32f30x.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must + * be called whenever the core clock is changed + * during program execution. + * + * 2. After each device reset the HSI (8 MHz) is used as system clock source. + * Then SystemInit() function is called, in "startup_stm32f30x.s" file, to + * configure the system clock before to branch to main program. + * + * 3. If the system clock source selected by user fails to startup, the SystemInit() + * function will do nothing and HSI still used as system clock source. User can + * add some code to deal with this issue inside the SetSysClock() function. + * + * 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define + * in "stm32f30x.h" file. When HSE is used as system clock source, directly or + * through PLL, and you are using different crystal you have to adapt the HSE + * value to your own configuration. + * + * 5. This file configures the system clock as follows: + *============================================================================= + * Supported STM32F30x device + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 2 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 2 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 8000000 + *---------------------------------------------------------------------------- + * PLLMUL | 9 + *----------------------------------------------------------------------------- + * PREDIV | 1 + *----------------------------------------------------------------------------- + * USB Clock | ENABLE + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 2 + *----------------------------------------------------------------------------- + * Prefetch Buffer | ON + *----------------------------------------------------------------------------- + *============================================================================= + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** @addtogroup STM32F30x_System_Private_Includes + * @{ + */ + +#include "stm32f30x.h" + +uint32_t hse_value = HSE_VALUE; + +/** + * @} + */ + +/* Private typedef -----------------------------------------------------------*/ + +/** @addtogroup STM32F30x_System_Private_Defines + * @{ + */ +/*!< Uncomment the following line if you need to relocate your vector Table in + Internal SRAM. */ +/* #define VECT_TAB_SRAM */ +#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +/** + * @} + */ + +/* Private macro -------------------------------------------------------------*/ + +/** @addtogroup STM32F30x_System_Private_Variables + * @{ + */ + + uint32_t SystemCoreClock = 72000000; + + __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_FunctionPrototypes + * @{ + */ + +void SetSysClock(void); + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_Functions + * @{ + */ + +/** + * @brief Setup the microcontroller system + * Initialize the Embedded Flash Interface, the PLL and update the + * SystemFrequency variable. + * @param None + * @retval None + */ +void SystemInit(void) +{ + /* FPU settings ------------------------------------------------------------*/ + #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ + #endif + + /* Reset the RCC clock configuration to the default reset state ------------*/ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001; + + /* Reset CFGR register */ + RCC->CFGR &= 0xF87FC00C; + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFF; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; + + /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */ + RCC->CFGR &= (uint32_t)0xFF80FFFF; + + /* Reset PREDIV1[3:0] bits */ + RCC->CFGR2 &= (uint32_t)0xFFFFFFF0; + + /* Reset USARTSW[1:0], I2CSW and TIMs bits */ + RCC->CFGR3 &= (uint32_t)0xFF00FCCC; + + /* Disable all interrupts */ + RCC->CIR = 0x00000000; + + /* Configure the System clock source, PLL Multiplier and Divider factors, + AHB/APBx prescalers and Flash settings ----------------------------------*/ + //SetSysClock(); // called from main() + +#ifdef VECT_TAB_SRAM + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */ +#else + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */ +#endif +} + +/** + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * or HSI_VALUE(*) multiplied/divided by the PLL factors. + * + * (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz), user has to ensure that HSE_VALUE is same as the real + * frequency of the crystal used. Otherwise, this function may + * have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param None + * @retval None + */ +void SystemCoreClockUpdate (void) +{ + uint32_t tmp = 0, pllmull = 0, pllsource = 0, prediv1factor = 0; + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case 0x00: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + case 0x04: /* HSE used as system clock */ + SystemCoreClock = HSE_VALUE; + break; + case 0x08: /* PLL used as system clock */ + /* Get PLL clock source and multiplication factor ----------------------*/ + pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; + pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; + pllmull = ( pllmull >> 18) + 2; + + if (pllsource == 0x00) + { + /* HSI oscillator clock divided by 2 selected as PLL clock entry */ + SystemCoreClock = (HSI_VALUE >> 1) * pllmull; + } + else + { + prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1; + /* HSE oscillator clock selected as PREDIV1 clock entry */ + SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; + } + break; + default: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK clock frequency ----------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; +} + +/** + * @brief Configures the System clock source, PLL Multiplier and Divider factors, + * AHB/APBx prescalers and Flash settings + * @note This function should be called only once the RCC clock configuration + * is reset to the default reset state (done in SystemInit() function). + * @param None + * @retval None + */ +void SetSysClock(void) +{ + __IO uint32_t StartUpCounter = 0, HSEStatus = 0; + +/******************************************************************************/ +/* PLL (clocked by HSE) used as System clock source */ +/******************************************************************************/ + + /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/ + /* Enable HSE */ + RCC->CR |= ((uint32_t)RCC_CR_HSEON); + + /* Wait till HSE is ready and if Time out is reached exit */ + do + { + HSEStatus = RCC->CR & RCC_CR_HSERDY; + StartUpCounter++; + } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + + if ((RCC->CR & RCC_CR_HSERDY) != RESET) + { + HSEStatus = (uint32_t)0x01; + } + else + { + HSEStatus = (uint32_t)0x00; + } + + if (HSEStatus == (uint32_t)0x01) + { + /* Enable Prefetch Buffer and set Flash Latency */ + FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1; + + /* HCLK = SYSCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1; + + /* PCLK2 = HCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1; + + /* PCLK1 = HCLK / 2 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2; + + /* PLL configuration */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL)); + RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9); + + /* Enable PLL */ + RCC->CR |= RCC_CR_PLLON; + + /* Wait till PLL is ready */ + while((RCC->CR & RCC_CR_PLLRDY) == 0) + { + } + + /* Select PLL as system clock source */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); + RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL; + + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL) + { + } + } + else + { /* If HSE fails to start-up, the application will have wrong clock + configuration. User can add here some code to deal with this error */ + } +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ + diff --git a/src/main/target/DOGE/system_stm32f30x.h b/src/main/target/DOGE/system_stm32f30x.h new file mode 100644 index 000000000..4f999d305 --- /dev/null +++ b/src/main/target/DOGE/system_stm32f30x.h @@ -0,0 +1,76 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.h + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** + * @brief Define to prevent recursive inclusion + */ +#ifndef __SYSTEM_STM32F30X_H +#define __SYSTEM_STM32F30X_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Exported types ------------------------------------------------------------*/ +extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/** @addtogroup STM32F30x_System_Exported_Functions + * @{ + */ + +extern void SystemInit(void); +extern void SystemCoreClockUpdate(void); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /*__SYSTEM_STM32F30X_H */ + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h new file mode 100644 index 000000000..3bb66f7fb --- /dev/null +++ b/src/main/target/DOGE/target.h @@ -0,0 +1,206 @@ +/* + * 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 "DOGE" + +// tqfp48 pin 34 +#define LED0_GPIO GPIOA +#define LED0_PIN Pin_13 +#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOA + +// tqfp48 pin 37 +#define LED1_GPIO GPIOA +#define LED1_PIN Pin_14 +#define LED1_PERIPHERAL RCC_AHBPeriph_GPIOA + +// tqfp48 pin 38 +#define LED2_GPIO GPIOA +#define LED2_PIN Pin_15 +#define LED2_PERIPHERAL RCC_AHBPeriph_GPIOA + +#define BEEP_GPIO GPIOB +#define BEEP_PIN Pin_2 +#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOB +//#define BEEPER_INVERTED + +// #define BEEP_GPIO GPIOB +// #define BEEP_PIN Pin_13 +// #define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOB +// #define BEEPER_INVERTED + +// tqfp48 pin 3 +#define MPU6500_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOC +#define MPU6500_CS_GPIO GPIOC +#define MPU6500_CS_PIN GPIO_Pin_14 +#define MPU6500_SPI_INSTANCE SPI1 + +// tqfp48 pin 25 +#define BMP280_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOB +#define BMP280_CS_GPIO GPIOB +#define BMP280_CS_PIN GPIO_Pin_12 +#define BMP280_SPI_INSTANCE SPI2 + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 + +#define SPI1_GPIO GPIOB +#define SPI1_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB +// tqfp48 pin 39 +#define SPI1_SCK_PIN GPIO_Pin_3 +#define SPI1_SCK_PIN_SOURCE GPIO_PinSource3 +// tqfp48 pin 40 +#define SPI1_MISO_PIN GPIO_Pin_4 +#define SPI1_MISO_PIN_SOURCE GPIO_PinSource4 +// tqfp48 pin 41 +#define SPI1_MOSI_PIN GPIO_Pin_5 +#define SPI1_MOSI_PIN_SOURCE GPIO_PinSource5 + +#define SPI2_GPIO GPIOB +#define SPI2_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB +// tqfp48 pin 26 +#define SPI2_SCK_PIN GPIO_Pin_13 +#define SPI2_SCK_PIN_SOURCE GPIO_PinSource13 +// tqfp48 pin 27 +#define SPI2_MISO_PIN GPIO_Pin_14 +#define SPI2_MISO_PIN_SOURCE GPIO_PinSource14 +// tqfp48 pin 28 +#define SPI2_MOSI_PIN GPIO_Pin_15 +#define SPI2_MOSI_PIN_SOURCE GPIO_PinSource15 + +// timer definitions in drivers/timer.c +// channel mapping in drivers/pwm_mapping.c +// only 6 outputs available on hardware +#define USABLE_TIMER_CHANNEL_COUNT 9 + +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + +#define GYRO +// #define USE_FAKE_GYRO +#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6500 +#define GYRO_MPU6500_ALIGN CW270_DEG // ?? + +#define ACC +// #define USE_FAKE_ACC +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 +#define ACC_MPU6500_ALIGN CW270_DEG // ?? + +#define BARO +#define USE_BARO_BMP280 +#define USE_BARO_SPI_BMP280 + +#define BEEPER +#define LED0 +#define LED1 +#define LED2 + +#define USB_IO +#define USE_VCP +#define USE_USART1 +#define USE_USART2 +#define USE_USART3 +#define SERIAL_PORT_COUNT 4 + +// tqfp48 pin 42 +#define UART1_TX_PIN GPIO_Pin_6 +// tqfp48 pin 43 +#define UART1_RX_PIN GPIO_Pin_7 +#define UART1_GPIO GPIOB +#define UART1_GPIO_AF GPIO_AF_7 +#define UART1_TX_PINSOURCE GPIO_PinSource6 +#define UART1_RX_PINSOURCE GPIO_PinSource7 + +// tqfp48 pin 12 +#define UART2_TX_PIN GPIO_Pin_2 +// tqfp48 pin 13 +#define UART2_RX_PIN GPIO_Pin_3 +#define UART2_GPIO GPIOA +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource2 +#define UART2_RX_PINSOURCE GPIO_PinSource3 + +// tqfp48 pin 21 +#define UART3_TX_PIN GPIO_Pin_10 +// tqfp48 pin 22 +#define UART3_RX_PIN GPIO_Pin_11 +#define UART3_GPIO GPIOB +#define UART3_GPIO_AF GPIO_AF_7 +#define UART3_TX_PINSOURCE GPIO_PinSource10 +#define UART3_RX_PINSOURCE GPIO_PinSource11 + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER + +#define ADC_INSTANCE ADC2 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 +#define ADC_DMA_CHANNEL DMA2_Channel1 + +// tqfp48 pin 14 +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_CHANNEL ADC_Channel_1 + +// tqfp48 pin 15 +#define CURRENT_METER_ADC_GPIO GPIOA +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 + +// mpu_int definition in sensors/initialisation.c +//#define DEBUG_MPU_DATA_READY_INTERRUPT +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready + +#define BLACKBOX +#define GPS +//#define GTUNE +#define LED_STRIP + +// tqfp48 pin 16 +#define LED_STRIP_TIMER TIM16 +#define USE_LED_STRIP_ON_DMA1_CHANNEL3 +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_GPIO_AF GPIO_AF_1 +#define WS2811_PIN GPIO_Pin_6 // TIM16_CH1 +#define WS2811_PIN_SOURCE GPIO_PinSource6 +#define WS2811_TIMER TIM16 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 +#define WS2811_DMA_CHANNEL DMA1_Channel3 +#define WS2811_IRQ DMA1_Channel3_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER + +#define TELEMETRY +#define SERIAL_RX +#define USE_SERVOS +#define USE_CLI + +#define SPEKTRUM_BIND +// Use UART3 for speksat +#define BIND_PORT GPIOB +#define BIND_PIN Pin_11 + +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + From 0600b2be951952878b018f8be49513625eef6eeb Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 8 Apr 2016 23:03:20 +0200 Subject: [PATCH 47/78] DOGE additional config --- fake_travis_build.sh | 3 ++- src/main/io/serial_msp.c | 2 +- src/main/target/DOGE/target.h | 4 +--- top_makefile | 2 ++ 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/fake_travis_build.sh b/fake_travis_build.sh index e7b9082f1..2a6a9b275 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -15,7 +15,8 @@ targets=("PUBLISHMETA=True" \ "TARGET=MOTOLAB" \ "TARGET=IRCFUSIONF3" \ "TARGET=ALIENFLIGHTF1" \ - "TARGET=ALIENFLIGHTF3") + "TARGET=ALIENFLIGHTF3" \ + "TARGET=DOGE") #fake a travis build environment export TRAVIS_BUILD_NUMBER=$(date +%s) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 65a9a0666..3561b2016 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -124,7 +124,7 @@ void setGyroSamplingSpeed(uint16_t looptime) { masterConfig.mag_hardware = 1; masterConfig.pid_process_denom = 2; } else if (looptime < 375) { -#if defined(LUX_RACE) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(ALIENFLIGHTF3) || defined(SPRACINGF3EVO) +#if defined(LUX_RACE) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(ALIENFLIGHTF3) || defined(SPRACINGF3EVO) || defined(DOGE) masterConfig.acc_hardware = 0; #else masterConfig.acc_hardware = 1; diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 3bb66f7fb..9be5650a7 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -201,6 +201,4 @@ #define BIND_PORT GPIOB #define BIND_PIN Pin_11 -#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#define USE_SERIAL_4WAY_SK_BOOTLOADER - +#define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/top_makefile b/top_makefile index 50385bfa3..4e1593fe0 100644 --- a/top_makefile +++ b/top_makefile @@ -13,6 +13,7 @@ ALL_TARGETS += motolab ALL_TARGETS += rmdo ALL_TARGETS += ircfusionf3 ALL_TARGETS += afromini +ALL_TARGETS += doge CLEAN_TARGETS := $(addprefix clean_, $(ALL_TARGETS)) @@ -31,6 +32,7 @@ clean_motolab motolab : opts := TARGET=MOTOLAB clean_rmdo rmdo : opts := TARGET=RMDO clean_ircfusionf3 ircfusionf3 : opts := TARGET=IRCFUSIONF3 clean_afromini afromini : opts := TARGET=AFROMINI +clean_afromini doge : opts := TARGET=DOGE .PHONY: all clean From bdaf8ecbb96549f76ba85b29485be5e5c3f6aca6 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 8 Apr 2016 23:28:37 +0200 Subject: [PATCH 48/78] New Horion default --- src/main/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 331165bd1..7cbc76148 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -174,7 +174,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[PIDMAG] = 40; pidProfile->P8[PIDVEL] = 55; pidProfile->I8[PIDVEL] = 55; - pidProfile->D8[PIDVEL] = 0; + pidProfile->D8[PIDVEL] = 75; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 70.0f; From 9000decdf7349413ed5d957b0b9562a9a99e6a60 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 15 Apr 2016 15:54:15 +0200 Subject: [PATCH 49/78] Fix SPI speed for MPU6000 for CC3D target and some new targets --- src/main/drivers/accgyro_spi_mpu6000.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 283c047a3..c816bca9a 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -132,6 +132,8 @@ void mpu6000SpiGyroInit(uint8_t lpf) mpu6000WriteRegister(MPU6000_CONFIG, lpf); delayMicroseconds(1); + spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock + int16_t data[3]; mpuGyroRead(data); From 4186ceaf81ec7bd14374a7445c96ab29acd9c109 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 16 Apr 2016 01:10:26 +0200 Subject: [PATCH 50/78] Fix SPRACINGF3EVO target --- src/main/target/SPRACINGF3EVO/target.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 27c094834..4586dbc2d 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -40,11 +40,11 @@ #define GYRO -//#define USE_FAKE_GYRO +#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 #define ACC -//#define USE_FAKE_ACC +#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 #define ACC_MPU6500_ALIGN CW180_DEG @@ -53,12 +53,12 @@ #define BARO #define USE_BARO_BMP280 -#define MAG -#define USE_MPU9250_MAG // Enables bypass configuration -#define USE_MAG_AK8963 +//#define MAG +//#define USE_MPU9250_MAG // Enables bypass configuration +//#define USE_MAG_AK8963 //#define USE_MAG_HMC5883 // External -#define MAG_AK8963_ALIGN CW90_DEG_FLIP +//#define MAG_AK8963_ALIGN CW90_DEG_FLIP //#define SONAR #define BEEPER From 587ffcfbc03b4f947e737bc97aedb06facf2a2d2 Mon Sep 17 00:00:00 2001 From: "Larry (TBS)" Date: Sat, 16 Apr 2016 18:28:18 +0800 Subject: [PATCH 51/78] - Enable MPU6000 --- Makefile | 1 + src/main/target/COLIBRI_RACE/target.h | 9 +++++++++ 2 files changed, 10 insertions(+) diff --git a/Makefile b/Makefile index 9bbf85ea5..4b6ace084 100644 --- a/Makefile +++ b/Makefile @@ -599,6 +599,7 @@ COLIBRI_RACE_SRC = \ drivers/bus_bst_stm32f30x.c \ drivers/accgyro_mpu.c \ drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6000.c \ drivers/accgyro_spi_mpu6500.c \ drivers/accgyro_mpu6500.c \ drivers/barometer_ms5611.c \ diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 4b9f5c634..4d9a06df9 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -43,6 +43,11 @@ #define MPU6500_CS_PIN GPIO_Pin_4 #define MPU6500_SPI_INSTANCE SPI1 +#define MPU6000_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOA +#define MPU6000_CS_GPIO GPIOA +#define MPU6000_CS_PIN GPIO_Pin_4 +#define MPU6000_SPI_INSTANCE SPI1 + #define USE_SPI #define USE_SPI_DEVICE_1 @@ -60,11 +65,15 @@ #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define GYRO +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW270_DEG #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 #define GYRO_MPU6500_ALIGN CW270_DEG #define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW270_DEG #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 #define ACC_MPU6500_ALIGN CW270_DEG From 57c6f62590bb37b0cf641f631d581c5e6db58cbc Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Sat, 16 Apr 2016 16:00:33 +0100 Subject: [PATCH 52/78] Added rate fields to log header Add Roll/Pitch/Yaw Rates and Expo to log header, add loop time to header --- src/main/blackbox/blackbox.c | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 06c6e34ae..23f24411e 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -323,6 +323,9 @@ extern uint32_t currentTime; //From rx.c: extern uint16_t rssi; +//From gyro.c +extern uint32_t targetLooptime; + static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED; static uint32_t blackboxLastArmingBeep = 0; @@ -1166,6 +1169,21 @@ static bool blackboxWriteSysinfo() blackboxPrintfHeaderLine("currentMeter:%d,%d", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale); } break; + case 13: + blackboxPrintfHeaderLine("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8); + break; + case 14: + blackboxPrintfHeaderLine("rcYawExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawExpo8); + break; + case 15: + blackboxPrintfHeaderLine("rates:%d,%d,%d", + masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[0], + masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[1], + masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[2]); + break; + case 16: + blackboxPrintfHeaderLine("looptime:%d", targetLooptime); + break; default: return true; } From c5bd5d687f2f75b0190b3499ef13ab240786a11e Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Sat, 16 Apr 2016 19:06:01 +0100 Subject: [PATCH 53/78] Add the Flightmode events Log RC mode selections to blackbox so that air mode, super expo, beeper on/off etc can be shown in blackbox viewer. --- src/main/blackbox/blackbox.c | 27 ++++++++++++++++++++++++++ src/main/blackbox/blackbox_fielddefs.h | 7 +++++++ 2 files changed, 34 insertions(+) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 23f24411e..1ffa5998c 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -326,9 +326,14 @@ extern uint16_t rssi; //From gyro.c extern uint32_t targetLooptime; +//From rc_controls.c +extern uint32_t rcModeActivationMask; + static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED; static uint32_t blackboxLastArmingBeep = 0; +static uint32_t blackboxLastFlightModeFlags = 0; // New event tracking of flight modes + static struct { uint32_t headerIndex; @@ -860,6 +865,8 @@ void startBlackbox(void) * it finally plays the beep for this arming event. */ blackboxLastArmingBeep = getArmingBeepTimeMicros(); + blackboxLastFlightModeFlags = rcModeActivationMask; // record startup status + blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE); } @@ -1211,6 +1218,10 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) case FLIGHT_LOG_EVENT_SYNC_BEEP: blackboxWriteUnsignedVB(data->syncBeep.time); break; + case FLIGHT_LOG_EVENT_FLIGHTMODE: // New flightmode flags write + blackboxWriteUnsignedVB(data->flightMode.flags); + blackboxWriteUnsignedVB(data->flightMode.lastFlags); + break; case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT: if (data->inflightAdjustment.floatFlag) { blackboxWrite(data->inflightAdjustment.adjustmentFunction + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG); @@ -1251,6 +1262,21 @@ static void blackboxCheckAndLogArmingBeep() } } +/* monitor the flight mode event status and trigger an event record if the state changes */ +static void blackboxCheckAndLogFlightMode() +{ + flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags + + // Use != so that we can still detect a change if the counter wraps + if (rcModeActivationMask != blackboxLastFlightModeFlags) { + eventData.lastFlags = blackboxLastFlightModeFlags; + blackboxLastFlightModeFlags = rcModeActivationMask; + eventData.flags = rcModeActivationMask; + + blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *) &eventData); + } +} + /* * Use the user's num/denom settings to decide if the P-frame of the given index should be logged, allowing the user to control * the portion of logged loop iterations. @@ -1295,6 +1321,7 @@ static void blackboxLogIteration() writeIntraframe(); } else { blackboxCheckAndLogArmingBeep(); + blackboxCheckAndLogFlightMode(); // Check for FlightMode status change event if (blackboxShouldLogPFrame(blackboxPFrameIndex)) { /* diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 0bee570f2..be6bc3e61 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -106,6 +106,7 @@ typedef enum FlightLogEvent { FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13, FLIGHT_LOG_EVENT_LOGGING_RESUME = 14, FLIGHT_LOG_EVENT_GTUNE_RESULT = 20, + FLIGHT_LOG_EVENT_FLIGHTMODE = 30, // Add new event type for flight mode status. FLIGHT_LOG_EVENT_LOG_END = 255 } FlightLogEvent; @@ -113,6 +114,11 @@ typedef struct flightLogEvent_syncBeep_s { uint32_t time; } flightLogEvent_syncBeep_t; +typedef struct flightLogEvent_flightMode_s { // New Event Data type + uint32_t flags; + uint32_t lastFlags; +} flightLogEvent_flightMode_t; + typedef struct flightLogEvent_inflightAdjustment_s { uint8_t adjustmentFunction; bool floatFlag; @@ -135,6 +141,7 @@ typedef struct flightLogEvent_gtuneCycleResult_s { typedef union flightLogEventData_u { flightLogEvent_syncBeep_t syncBeep; + flightLogEvent_flightMode_t flightMode; // New event data flightLogEvent_inflightAdjustment_t inflightAdjustment; flightLogEvent_loggingResume_t loggingResume; flightLogEvent_gtuneCycleResult_t gtuneCycleResult; From d4228388afda12559eb52c16c5b135db64d37983 Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Sat, 16 Apr 2016 19:28:29 +0100 Subject: [PATCH 54/78] Add SuperExpo Factor --- src/main/blackbox/blackbox.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 1ffa5998c..e5001930d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1183,12 +1183,15 @@ static bool blackboxWriteSysinfo() blackboxPrintfHeaderLine("rcYawExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawExpo8); break; case 15: + blackboxPrintfHeaderLine("superExpoFactor:%d", masterConfig.rxConfig.superExpoFactor); + break; + case 16: blackboxPrintfHeaderLine("rates:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[0], masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[1], masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[2]); break; - case 16: + case 17: blackboxPrintfHeaderLine("looptime:%d", targetLooptime); break; default: From 6ce8fe3c0fa7725e77d6ea61557bcec65d6cdb34 Mon Sep 17 00:00:00 2001 From: Beaky2000 Date: Sun, 17 Apr 2016 22:09:49 +0200 Subject: [PATCH 55/78] Fixed a bug which caused printing of floats larger than 100 in the CLI to crash --- src/main/common/typeconversion.c | 5 +++++ src/main/common/typeconversion.h | 10 ++++++++++ src/main/io/serial_cli.c | 2 +- 3 files changed, 16 insertions(+), 1 deletion(-) diff --git a/src/main/common/typeconversion.c b/src/main/common/typeconversion.c index 4e3f8b4c2..3a3730fd5 100644 --- a/src/main/common/typeconversion.c +++ b/src/main/common/typeconversion.c @@ -144,6 +144,11 @@ char *itoa(int i, char *a, int base) #endif +/* Note: the floatString must be at least 13 bytes long to cover all cases. + * This includes up to 10 digits (32 bit ints can hold numbers up to 10 + * digits long) + the decimal point + negative sign or space + null + * terminator. + */ char *ftoa(float x, char *floatString) { int32_t value; diff --git a/src/main/common/typeconversion.h b/src/main/common/typeconversion.h index 62b437875..f2811d65b 100644 --- a/src/main/common/typeconversion.h +++ b/src/main/common/typeconversion.h @@ -21,7 +21,17 @@ void li2a(long num, char *bf); void ui2a(unsigned int num, unsigned int base, int uc, char *bf); void i2a(int num, char *bf); char a2i(char ch, const char **src, int base, int *nump); + +/* Simple conversion of a float to a string. Will display completely + * inaccurate results for floats larger than about 2.15E6 (2^31 / 1000) + * (same thing for negative values < -2.15E6). + * Will always display 3 decimals, so anything smaller than 1E-3 will + * not be displayed. + * The floatString will be filled in with the result and will be + * returned. It must be at least 13 bytes in length to cover all cases! + */ char *ftoa(float x, char *floatString); + float fastA2F(const char *p); #ifndef HAVE_ITOA_FUNCTION diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 7106a99fb..8f009328f 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2446,7 +2446,7 @@ static void cliWrite(uint8_t ch) static void cliPrintVar(const clivalue_t *var, uint32_t full) { int32_t value = 0; - char buf[8]; + char buf[13]; void *ptr = var->ptr; if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { From fb35cc647750e29d14d9a7d3db6672b15ce602df Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 16 Apr 2016 23:35:02 +0200 Subject: [PATCH 56/78] Remove MW23 pid controller --- src/main/flight/pid.c | 144 +-------------------------------------- src/main/flight/pid.h | 3 +- src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.c | 2 +- src/main/mw.c | 17 ++--- 5 files changed, 9 insertions(+), 159 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 044289ee0..3809062c1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -59,7 +59,7 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; +uint8_t PIDweight[3]; static int32_t errorGyroI[3], errorGyroILimit[3]; static float errorGyroIf[3], errorGyroIfLimit[3]; @@ -273,145 +273,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } } -static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, - rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) -{ - UNUSED(rxConfig); - - int axis, prop = 0; - int32_t rc, error, errorAngle, delta, gyroError; - int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm; - static int16_t lastErrorForDelta[2]; - static int32_t deltaState[3][DELTA_MAX_SAMPLES]; - - if (FLIGHT_MODE(HORIZON_MODE)) { - prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512); - } - - // PITCH & ROLL - for (axis = 0; axis < 2; axis++) { - - rc = rcCommand[axis] << 1; - - gyroError = gyroADC[axis] / 4; - - error = rc - gyroError; - errorGyroI[axis] = constrain(errorGyroI[axis] + ((error * (uint16_t)targetPidLooptime) >> 11) , -16000, +16000); // WindUp 16 bits is ok here - - if (ABS(gyroADC[axis]) > (640 * 4)) { - errorGyroI[axis] = 0; - } - - // Anti windup protection - if (antiWindupProtection || motorLimitReached) { - errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); - } else { - errorGyroILimit[axis] = ABS(errorGyroI[axis]); - } - - ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 - - PTerm = (int32_t)rc * pidProfile->P8[axis] >> 6; - - if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // axis relying on ACC - // 50 degrees max inclination -#ifdef GPS - errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#else - errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#endif - - errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp //16 bits is ok here - - PTermACC = ((int32_t)errorAngle * pidProfile->P8[PIDLEVEL]) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result - - int16_t limit = pidProfile->D8[PIDLEVEL] * 5; - PTermACC = constrain(PTermACC, -limit, +limit); - - ITermACC = ((int32_t)errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result - - ITerm = ITermACC + ((ITerm - ITermACC) * prop >> 9); - PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9); - } - - PTerm -= ((int32_t)gyroError * dynP8[axis]) >> 6; // 32 bits is needed for calculation - - //-----calculate D-term based on the configured approach (delta from measurement or deltafromError) - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = error - lastErrorForDelta[axis]; - lastErrorForDelta[axis] = error; - } else { /* Delta from measurement */ - delta = -(gyroError - lastErrorForDelta[axis]); - lastErrorForDelta[axis] = gyroError; - } - - // Scale delta to looptime - delta = (delta * ((uint16_t) 0xFFFF)) / ((uint16_t)targetPidLooptime << 5); - - // Filer delta - if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); - - // Apply moving average and multiply to get original scaling - if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2; - - DTerm = (delta * dynD8[axis]) >> 5; // 32 bits is needed for calculation - - axisPID[axis] = PTerm + ITerm + DTerm; - - if (lowThrottlePidReduction) axisPID[axis] /= 3; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } - - //YAW - rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30) >> 5; -#ifdef ALIENWII32 - error = rc - gyroADC[FD_YAW]; -#else - error = rc - (gyroADC[FD_YAW] / 4); -#endif - errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; - errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); - if (ABS(rc) > 50) errorGyroI[FD_YAW] = 0; - - PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // TODO: Bitwise shift on a signed integer is not recommended - - // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply - if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) { - PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); - } - - ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); - - axisPID[FD_YAW] = PTerm + ITerm; - - if (pidProfile->yaw_lpf_hz) axisPID[FD_YAW] = filterApplyPt1(axisPID[FD_YAW], &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(FD_YAW); - } -#endif - -#ifdef BLACKBOX - axisPID_P[FD_YAW] = PTerm; - axisPID_I[FD_YAW] = ITerm; - axisPID_D[FD_YAW] = 0; -#endif -} - static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { @@ -567,9 +428,6 @@ void pidSetController(pidControllerType_e type) break; case PID_CONTROLLER_LUX_FLOAT: pid_controller = pidLuxFloat; - break; - case PID_CONTROLLER_MW23: - pid_controller = pidMultiWii23; } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4208ad6c4..dfcce336f 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -37,8 +37,7 @@ typedef enum { } pidIndex_e; typedef enum { - PID_CONTROLLER_MW23, - PID_CONTROLLER_MWREWRITE, + PID_CONTROLLER_MWREWRITE = 1, PID_CONTROLLER_LUX_FLOAT, PID_COUNT } pidControllerType_e; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 7106a99fb..b2f19e1c8 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -371,7 +371,7 @@ static const char * const lookupTableBlackboxDevice[] = { static const char * const lookupTablePidController[] = { - "MW23", "MWREWRITE", "LUX" + "UNUSED", "MWREWRITE", "LUX" }; static const char * const lookupTableSerialRX[] = { diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 3561b2016..c78212430 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1309,7 +1309,7 @@ static bool processInCommand(void) break; case MSP_SET_PID_CONTROLLER: oldPid = currentProfile->pidProfile.pidController; - currentProfile->pidProfile.pidController = read8(); + currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); pidSetController(currentProfile->pidProfile.pidController); if (oldPid != currentProfile->pidProfile.pidController) setGyroSamplingSpeed(0); // recalculate looptimes for new PID break; diff --git a/src/main/mw.c b/src/main/mw.c index 11149dc04..7f7497f6b 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -223,16 +223,16 @@ void scaleRcCommandToFpvCamAngle(void) { void annexCode(void) { int32_t tmp, tmp2; - int32_t axis, prop1 = 0, prop2; + int32_t axis, prop; // PITCH & ROLL only dynamic PID adjustment, depending on throttle value if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { - prop2 = 100; + prop = 100; } else { if (rcData[THROTTLE] < 2000) { - prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); + prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); } else { - prop2 = 100 - currentControlRateProfile->dynThrPID; + prop = 100 - currentControlRateProfile->dynThrPID; } } @@ -249,8 +249,6 @@ void annexCode(void) tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; - prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; - prop1 = (uint16_t)prop1 * prop2 / 100; } else if (axis == YAW) { if (masterConfig.rcControlsConfig.yaw_deadband) { if (tmp > masterConfig.rcControlsConfig.yaw_deadband) { @@ -261,19 +259,14 @@ void annexCode(void) } tmp2 = tmp / 100; rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction; - prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; } - // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. - dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100; - dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100; - dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100; // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids if (axis == YAW) { PIDweight[axis] = 100; } else { - PIDweight[axis] = prop2; + PIDweight[axis] = prop; } if (rcData[axis] < masterConfig.rxConfig.midrc) From c92f511b05ff587167aa74d32fce0b67428111ee Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 16 Apr 2016 23:47:10 +0200 Subject: [PATCH 57/78] Match rewrite and luxfloat delta scaling during averaging --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3809062c1..b54b058b5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -249,7 +249,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); // Apply moving average - if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]); + if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2; DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f); } From 1531c07687759c5c81867ee957fd1f64890e31ed Mon Sep 17 00:00:00 2001 From: "Larry (TBS)" Date: Thu, 31 Mar 2016 17:50:36 +0800 Subject: [PATCH 58/78] - Add defulat LED Strip values for Colibri Race --- src/main/io/ledstrip.c | 13 +++++++++++++ src/main/target/COLIBRI_RACE/target.h | 1 + 2 files changed, 14 insertions(+) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 701d29a0a..a52ed8df0 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -245,6 +245,19 @@ const ledConfig_t defaultLedStripConfig[] = { { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, }; +#elif defined(USE_COLIBTI_RACE_LED_DEFAULT_CONFIG) +const ledConfig_t defaultLedStripConfig[] = { + { CALCULATE_LED_XY( 0, 0), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 1), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 8), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 7, 15), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 8, 15), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 7, 14), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 8, 14), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 8), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 1), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 0), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, +}; #else const ledConfig_t defaultLedStripConfig[] = { { CALCULATE_LED_XY(15, 15), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 4b9f5c634..6d0669361 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -155,6 +155,7 @@ #define GPS //#define GTUNE #define LED_STRIP +#define USE_COLIBTI_RACE_LED_DEFAULT_CONFIG #define LED_STRIP_TIMER TIM16 From 1a5a49f9ecd4382dc7702231c8c337a6db7f8092 Mon Sep 17 00:00:00 2001 From: "Larry (TBS)" Date: Fri, 22 Apr 2016 17:03:47 +0800 Subject: [PATCH 59/78] - Enable MPU9250 hardware in Colibri Race --- Makefile | 1 + src/main/target/COLIBRI_RACE/target.h | 2 ++ 2 files changed, 3 insertions(+) diff --git a/Makefile b/Makefile index 9bbf85ea5..e916d3f5a 100644 --- a/Makefile +++ b/Makefile @@ -602,6 +602,7 @@ COLIBRI_RACE_SRC = \ drivers/accgyro_spi_mpu6500.c \ drivers/accgyro_mpu6500.c \ drivers/barometer_ms5611.c \ + drivers/compass_ak8963.c \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ drivers/display_ug2864hsweg01.c \ diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 4b9f5c634..71726750d 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -73,7 +73,9 @@ #define USE_BARO_MS5611 #define MAG +#define USE_MPU9250_MAG #define USE_MAG_HMC5883 +#define USE_MAG_AK8963 #define USE_MAG_AK8975 #define BEEPER From 1e1d445fd3ac76f8c328e79cd91218cedcac6c27 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 16 Apr 2016 23:16:56 +0200 Subject: [PATCH 60/78] Dynamic D Implementation --- src/main/config/config.c | 5 +++-- src/main/flight/pid.c | 24 +++++++++++++++++++++--- src/main/flight/pid.h | 1 + src/main/io/serial_cli.c | 1 + src/main/version.h | 2 +- 5 files changed, 27 insertions(+), 6 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 7cbc76148..ddf15f8c7 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 131; +static const uint8_t EEPROM_CONF_VERSION = 132; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -178,7 +178,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 70.0f; - pidProfile->dterm_average_count = 4; + pidProfile->dterm_average_count = 0; + pidProfile->dynamic_dterm_threshold = 20; pidProfile->rollPitchItermResetRate = 200; pidProfile->yawItermResetRate = 50; pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b54b058b5..a343d0c73 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -133,6 +133,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa static float lastErrorForDelta[3]; static float deltaState[3][DELTA_MAX_SAMPLES]; float delta; + float dynamicDTermGain; int axis; float horizonLevelStrength = 1; @@ -141,7 +142,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // Scaling factors for Pids to match rewrite and use same defaults static const float luxPTermScale = 1.0f / 128; static const float luxITermScale = 1000000.0f / 0x1000000; - static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 256; + static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 508; if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions @@ -245,6 +246,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // would be scaled by different dt each time. Division by dT fixes that. delta *= (1.0f / getdT()); + // Several different Dterm filtering methods to prevent noise. All of them can be combined + // Filter delta if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); @@ -252,6 +255,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2; DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f); + + // Dynamic D Implementation + if (pidProfile->dynamic_dterm_threshold) { + dynamicDTermGain = constrainf(ABS(DTerm) / pidProfile->dynamic_dterm_threshold, 0.0f, 1.0f); + DTerm *= dynamicDTermGain; + } } // -----calculate total PID output @@ -283,6 +292,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co static int32_t lastErrorForDelta[3] = { 0, 0, 0 }; static int32_t deltaState[3][DELTA_MAX_SAMPLES]; int32_t AngleRateTmp, RateError, gyroRate; + int32_t dynamicDTermGain; int8_t horizonLevelStrength = 100; @@ -389,15 +399,23 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // would be scaled by different dt each time. Division by dT fixes that. - delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 6; + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; + + // Several different Dterm filtering methods to prevent noise. All of them can be combined // Filter delta if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); // Apply moving average - if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2; + if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]); DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // Dynamic D Implementation + if (pidProfile->dynamic_dterm_threshold) { + dynamicDTermGain = constrain((ABS(DTerm) << 7) / pidProfile->dynamic_dterm_threshold, 0, 1 << 7); + DTerm = (DTerm * dynamicDTermGain) >> 7; + } } // -----calculate total PID output diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index dfcce336f..8fddd77bb 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -71,6 +71,7 @@ typedef struct pidProfile_s { uint8_t deltaMethod; // Alternative delta Calculation uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm + uint8_t dynamic_dterm_threshold; #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b2f19e1c8..d9611bb1f 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -722,6 +722,7 @@ const clivalue_t valueTable[] = { { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } }, + { "dynamic_dterm_threshold", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dynamic_dterm_threshold, .config.minmax = {0, 100 } }, { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, { "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, diff --git a/src/main/version.h b/src/main/version.h index 799c7d43b..587b442c7 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -17,7 +17,7 @@ #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed +#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) From f4219aebba589c3900cd66a1ba6f478f100613c6 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 22 Apr 2016 12:45:40 +0200 Subject: [PATCH 61/78] Fix top_makefile typo --- top_makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/top_makefile b/top_makefile index 4e1593fe0..a091319a6 100644 --- a/top_makefile +++ b/top_makefile @@ -32,7 +32,7 @@ clean_motolab motolab : opts := TARGET=MOTOLAB clean_rmdo rmdo : opts := TARGET=RMDO clean_ircfusionf3 ircfusionf3 : opts := TARGET=IRCFUSIONF3 clean_afromini afromini : opts := TARGET=AFROMINI -clean_afromini doge : opts := TARGET=DOGE +clean_doge doge : opts := TARGET=DOGE .PHONY: all clean From 372c9de6518c0f7bffcfbf76554d42c6086fd4cd Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Sun, 24 Apr 2016 21:52:35 +0100 Subject: [PATCH 62/78] Added PID configuration, filters etc to Log file Add the PID configuration settings, controller number, dterm filters etc to blackbox log file. --- src/main/blackbox/blackbox.c | 121 +++++++++++++++++++++++++++++++++-- 1 file changed, 115 insertions(+), 6 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index e5001930d..a8805dd9f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1183,17 +1183,126 @@ static bool blackboxWriteSysinfo() blackboxPrintfHeaderLine("rcYawExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawExpo8); break; case 15: - blackboxPrintfHeaderLine("superExpoFactor:%d", masterConfig.rxConfig.superExpoFactor); + blackboxPrintfHeaderLine("thrMid:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrMid8); break; case 16: - blackboxPrintfHeaderLine("rates:%d,%d,%d", - masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[0], - masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[1], - masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[2]); + blackboxPrintfHeaderLine("thrExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrExpo8); break; case 17: + blackboxPrintfHeaderLine("dynThrPID:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].dynThrPID); + break; + case 18: + blackboxPrintfHeaderLine("tpa_breakpoint:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].tpa_breakpoint); + break; + case 19: + blackboxPrintfHeaderLine("superExpoFactor:%d", masterConfig.rxConfig.superExpoFactor); + break; + case 20: + blackboxPrintfHeaderLine("rates:%d,%d,%d", + masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[ROLL], + masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[PITCH], + masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[YAW]); + break; + case 21: blackboxPrintfHeaderLine("looptime:%d", targetLooptime); break; + case 22: + blackboxPrintfHeaderLine("pidController:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidController); + break; + case 23: + blackboxPrintfHeaderLine("rollPID:%d,%d,%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[ROLL], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[ROLL], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[ROLL]); + break; + case 24: + blackboxPrintfHeaderLine("pitchPID:%d,%d,%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PITCH], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PITCH], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PITCH]); + break; + case 25: + blackboxPrintfHeaderLine("yawPID:%d,%d,%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[YAW], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[YAW], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[YAW]); + break; + case 26: + blackboxPrintfHeaderLine("altPID:%d,%d,%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDALT], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDALT], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDALT]); + break; + case 27: + blackboxPrintfHeaderLine("posPID:%d,%d,%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOS], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOS], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOS]); + break; + case 28: + blackboxPrintfHeaderLine("posrPID:%d,%d,%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOSR], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOSR], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOSR]); + break; + case 29: + blackboxPrintfHeaderLine("navrPID:%d,%d,%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDNAVR], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDNAVR], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDNAVR]); + break; + case 30: + blackboxPrintfHeaderLine("levelPID:%d,%d,%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]); + break; + case 31: + blackboxPrintfHeaderLine("magPID:%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDMAG]); + break; + case 32: + blackboxPrintfHeaderLine("velPID:%d,%d,%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDVEL], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDVEL], + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDVEL]); + break; + case 33: + blackboxPrintfHeaderLine("yaw_p_limit:%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit); + break; + case 34: + blackboxPrintfHeaderLine("yaw_lpf_hz:%d", + (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0)); + break; + case 35: + blackboxPrintfHeaderLine("dterm_average_count:%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); + break; + case 36: + blackboxPrintfHeaderLine("dynamic_dterm_threshold:%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_dterm_threshold); + break; + case 37: + blackboxPrintfHeaderLine("rollPitchItermResetRate:%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermResetRate); + break; + case 38: + blackboxPrintfHeaderLine("yawItermResetRate:%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermResetRate); + break; + case 39: + blackboxPrintfHeaderLine("dterm_lpf_hz:%d", + (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0)); + break; + case 40: + blackboxPrintfHeaderLine("deltaMethod:%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.deltaMethod); + break; + case 41: + blackboxPrintfHeaderLine("H_sensitivity:%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.H_sensitivity); + break; default: return true; } @@ -1519,5 +1628,5 @@ void initBlackbox(void) blackboxSetState(BLACKBOX_STATE_DISABLED); } } - #endif + From 6343da6f091dde3e5eec2cad1ac054840166a489 Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Mon, 25 Apr 2016 07:18:34 +0100 Subject: [PATCH 63/78] Use the flightModeFlags slow field for rcModes This value was always zero, so re-task it for the flight mode selection (which it was always designed for). --- src/main/blackbox/blackbox.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index a8805dd9f..0fae762e8 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -307,7 +307,7 @@ typedef struct blackboxGpsState_s { // This data is updated really infrequently: typedef struct blackboxSlowState_s { - uint16_t flightModeFlags; + uint32_t flightModeFlags; // extend this data size (from uint16_t) uint8_t stateFlags; uint8_t failsafePhase; bool rxSignalReceived; @@ -742,7 +742,7 @@ static void writeSlowFrame(void) */ static void loadSlowState(blackboxSlowState_t *slow) { - slow->flightModeFlags = flightModeFlags; + slow->flightModeFlags = rcModeActivationMask; //was flightModeFlags; slow->stateFlags = stateFlags; slow->failsafePhase = failsafePhase(); slow->rxSignalReceived = rxIsReceivingSignal(); From 3444967b1cfbb51ade43c8ee7ba191bda74485f5 Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Mon, 25 Apr 2016 13:31:51 +0100 Subject: [PATCH 64/78] Extend header to include setup information Add additional header information for PID parameters etc into header. --- src/main/blackbox/blackbox.c | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 0fae762e8..617d85800 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1303,6 +1303,42 @@ static bool blackboxWriteSysinfo() blackboxPrintfHeaderLine("H_sensitivity:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.H_sensitivity); break; + case 42: + blackboxPrintfHeaderLine("deadband:%d", masterConfig.rcControlsConfig.deadband); + break; + case 43: + blackboxPrintfHeaderLine("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); + break; + case 44: + blackboxPrintfHeaderLine("gyro_lpf:%d", masterConfig.gyro_lpf); + break; + case 45: + blackboxPrintfHeaderLine("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0)); + break; + case 46: + blackboxPrintfHeaderLine("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0)); + break; + case 47: + blackboxPrintfHeaderLine("acc_hardware:%d", masterConfig.acc_hardware); + break; + case 48: + blackboxPrintfHeaderLine("baro_hardware:%d", masterConfig.baro_hardware); + break; + case 49: + blackboxPrintfHeaderLine("mag_hardware:%d", masterConfig.mag_hardware); + break; + case 50: + blackboxPrintfHeaderLine("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm); + break; + case 51: + blackboxPrintfHeaderLine("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation); + break; + case 52: + blackboxPrintfHeaderLine("rc_smoothing:%d", masterConfig.rxConfig.rcSmoothing); + break; + case 53: + blackboxPrintfHeaderLine("features:%d", masterConfig.enabledFeatures); + break; default: return true; } From 5782da962653601ae16d11b84137e0edbd06d4f4 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 26 Apr 2016 18:58:05 +0200 Subject: [PATCH 65/78] top_makefile allways builds both .hex and .bin formats. MAin Makefile llways cleans up both fileformat too. --- Makefile | 5 +---- top_makefile | 4 ++-- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/Makefile b/Makefile index 4c3ebf560..90217b59a 100644 --- a/Makefile +++ b/Makefile @@ -879,11 +879,8 @@ TARGET_DEPS = $(addsuffix .d,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $( TARGET_MAP = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).map -ifeq ($(OPBL),yes) CLEAN_ARTIFACTS := $(TARGET_BIN) -else -CLEAN_ARTIFACTS := $(TARGET_HEX) -endif +CLEAN_ARTIFACTS += $(TARGET_HEX) CLEAN_ARTIFACTS += $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP) # List of buildable ELF files and their object dependencies. diff --git a/top_makefile b/top_makefile index a091319a6..d765dcf35 100644 --- a/top_makefile +++ b/top_makefile @@ -22,7 +22,7 @@ clean_cc3d cc3d: opts := TARGET=CC3D clean_cc3d_opbl cc3d_opbl : opts := TARGET=CC3D_OPBL clean_spracingf3mini spracingf3mini : opts := TARGET=SPRACINGF3MINI clean_spracingf3 spracingf3 : opts := TARGET=SPRACINGF3 -clean_spracingf3 spracingf3evo : opts := TARGET=SPRACINGF3EVO +clean_spracingf3evo spracingf3evo : opts := TARGET=SPRACINGF3EVO clean_sparky sparky : opts := TARGET=SPARKY clean_alienflightf1 alienflightf1 : opts := TARGET=ALIENFLIGHTF1 clean_alienflightf3 alienflightf3 : opts := TARGET=ALIENFLIGHTF3 @@ -49,7 +49,7 @@ everything: $(ALL_TARGETS) .PHONY:$(ALL_TARGETS) $(ALL_TARGETS): - make -f Makefile $(opts) + make -f Makefile hex binary $(opts) .PHONY: $(CLEAN_TARGETS) $(CLEAN_TARGETS): From cbcf0283028372911b0f9caef4efd25e3ba4c5aa Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 27 Apr 2016 21:08:15 +0200 Subject: [PATCH 66/78] Dterm robust differentiator Initial Implementation --- src/main/config/config.c | 2 +- src/main/flight/pid.c | 70 ++++++++++++++++------------------------ src/main/flight/pid.h | 6 +++- src/main/io/serial_cli.c | 3 +- 4 files changed, 35 insertions(+), 46 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index ddf15f8c7..ffb211137 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -179,7 +179,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 70.0f; pidProfile->dterm_average_count = 0; - pidProfile->dynamic_dterm_threshold = 20; + pidProfile->dterm_differentiator = 1; pidProfile->rollPitchItermResetRate = 200; pidProfile->yawItermResetRate = 50; pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a343d0c73..37014c05d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -130,10 +130,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa { float RateError, AngleRate, gyroRate; float ITerm,PTerm,DTerm; - static float lastErrorForDelta[3]; - static float deltaState[3][DELTA_MAX_SAMPLES]; + static float lastRate[3][PID_LAST_RATE_COUNT]; float delta; - float dynamicDTermGain; int axis; float horizonLevelStrength = 1; @@ -234,33 +232,28 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); DTerm = 0; } else { - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateError - lastErrorForDelta[axis]; - lastErrorForDelta[axis] = RateError; + if (pidProfile->dterm_differentiator) { + // Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters) + // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ + // N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4 + delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8; + for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) { + lastRate[axis][i] = lastRate[axis][i-1]; + } } else { - delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastErrorForDelta[axis] = gyroRate; + // When DTerm low pass filter disabled apply moving average to reduce noise + delta = -(gyroRate - lastRate[axis][0]); } - // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference - // would be scaled by different dt each time. Division by dT fixes that. - delta *= (1.0f / getdT()); + lastRate[axis][0] = gyroRate; - // Several different Dterm filtering methods to prevent noise. All of them can be combined + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta *= (1.0f / getdT()); // Filter delta if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); - // Apply moving average - if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2; - DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f); - - // Dynamic D Implementation - if (pidProfile->dynamic_dterm_threshold) { - dynamicDTermGain = constrainf(ABS(DTerm) / pidProfile->dynamic_dterm_threshold, 0.0f, 1.0f); - DTerm *= dynamicDTermGain; - } } // -----calculate total PID output @@ -289,10 +282,8 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co int axis; int32_t PTerm, ITerm, DTerm, delta; - static int32_t lastErrorForDelta[3] = { 0, 0, 0 }; - static int32_t deltaState[3][DELTA_MAX_SAMPLES]; + static int32_t lastRate[3][PID_LAST_RATE_COUNT]; int32_t AngleRateTmp, RateError, gyroRate; - int32_t dynamicDTermGain; int8_t horizonLevelStrength = 100; @@ -389,33 +380,28 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); DTerm = 0; } else { - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateError - lastErrorForDelta[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastErrorForDelta[axis] = RateError; + if (pidProfile->dterm_differentiator) { + // Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters) + // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ + // N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4 + delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8; + for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) { + lastRate[axis][i] = lastRate[axis][i-1]; + } } else { - delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastErrorForDelta[axis] = gyroRate; + // When DTerm low pass filter disabled apply moving average to reduce noise + delta = -(gyroRate - lastRate[axis][0]); } - // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference - // would be scaled by different dt each time. Division by dT fixes that. - delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; + lastRate[axis][0] = gyroRate; - // Several different Dterm filtering methods to prevent noise. All of them can be combined + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; // Filter delta if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); - // Apply moving average - if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]); - DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // Dynamic D Implementation - if (pidProfile->dynamic_dterm_threshold) { - dynamicDTermGain = constrain((ABS(DTerm) << 7) / pidProfile->dynamic_dterm_threshold, 0, 1 << 7); - DTerm = (DTerm * dynamicDTermGain) >> 7; - } } // -----calculate total PID output diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 8fddd77bb..2c190feb9 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -22,6 +22,10 @@ #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 400 // Maximum value for yaw P limiter +#define PID_LAST_RATE_COUNT 7 +#define PID_DTERM_FIR_MAX_LENGTH 7 +#define PID_MAX_DIFFERENTIATOR (PID_DTERM_FIR_MAX_LENGTH-2) + typedef enum { PIDROLL, PIDPITCH, @@ -71,7 +75,7 @@ typedef struct pidProfile_s { uint8_t deltaMethod; // Alternative delta Calculation uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm - uint8_t dynamic_dterm_threshold; + uint8_t dterm_differentiator; #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9100e03a5..550293fee 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -721,8 +721,7 @@ const clivalue_t valueTable[] = { { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, - { "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } }, - { "dynamic_dterm_threshold", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dynamic_dterm_threshold, .config.minmax = {0, 100 } }, + { "dterm_differentiator", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_differentiator, .config.lookup = { TABLE_OFF_ON } }, { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, { "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, From 86c2e12c079f454eb06e1f279ef7233d6c04795d Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 27 Apr 2016 22:09:17 +0200 Subject: [PATCH 67/78] Optional SUPER EXPO for yaw // Optional always Iterm reset // Rework Iterm reset --- src/main/blackbox/blackbox.c | 4 ++-- src/main/config/config.c | 6 ++++-- src/main/flight/pid.c | 28 +++++++++++++++++----------- src/main/flight/pid.h | 13 +++++++++++-- src/main/io/serial_cli.c | 11 ++++++++++- src/main/rx/rx.h | 2 ++ 6 files changed, 46 insertions(+), 18 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 617d85800..b0413f6bf 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1280,8 +1280,8 @@ static bool blackboxWriteSysinfo() masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); break; case 36: - blackboxPrintfHeaderLine("dynamic_dterm_threshold:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_dterm_threshold); + blackboxPrintfHeaderLine("dterm_differentiator:%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_differentiator); break; case 37: blackboxPrintfHeaderLine("rollPitchItermResetRate:%d", diff --git a/src/main/config/config.c b/src/main/config/config.c index ffb211137..822e95515 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 132; +static const uint8_t EEPROM_CONF_VERSION = 133; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -178,9 +178,9 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 70.0f; - pidProfile->dterm_average_count = 0; pidProfile->dterm_differentiator = 1; pidProfile->rollPitchItermResetRate = 200; + pidProfile->rollPitchItermResetAlways = 0; pidProfile->yawItermResetRate = 50; pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; @@ -454,6 +454,8 @@ static void resetConf(void) masterConfig.rxConfig.fpvCamAngleDegrees = 0; masterConfig.rxConfig.max_aux_channel = 6; masterConfig.rxConfig.superExpoFactor = 30; + masterConfig.rxConfig.superExpoFactorYaw = 30; + masterConfig.rxConfig.superExpoYawMode = 0; resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 37014c05d..0246fa71f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -80,11 +80,17 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) { } float calculateExpoPlus(int axis, rxConfig_t *rxConfig) { - float propFactor; + float propFactor; + float superExpoFactor; - propFactor = 1.0f - ((rxConfig->superExpoFactor / 100.0f) * (ABS(rcCommand[axis]) / 500.0f)); + if (axis == YAW && !rxConfig->superExpoYawMode) { + propFactor = 1.0f; + } else { + superExpoFactor = (axis == YAW) ? rxConfig->superExpoFactorYaw : rxConfig->superExpoFactor; + propFactor = 1.0f - ((superExpoFactor / 100.0f) * (ABS(rcCommand[axis]) / 500.0f)); + } - return propFactor; + return propFactor; } void pidResetErrorAngle(void) @@ -195,7 +201,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa RateError = AngleRate - gyroRate; // -----calculate P component - if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) { + if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { PTerm = (luxPTermScale * pidProfile->P8[axis] * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig)); } else { PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor; @@ -209,12 +215,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate I component. errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * pidProfile->I8[axis], -250.0f, 250.0f); - if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) { - if (ABS(gyroRate / 4.1f) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; + if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) { + if (ABS(gyroRate / 4.1f) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD); } if (axis == YAW) { - if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; + if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW); } if (antiWindupProtection || motorLimitReached) { @@ -337,7 +343,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co RateError = AngleRateTmp - gyroRate; // -----calculate P component - if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) { + if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { PTerm = (pidProfile->P8[axis] * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7; } else { PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; @@ -359,12 +365,12 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // I coefficient (I8) moved before integration to make limiting independent from PID settings errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); - if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) { - if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = 0; + if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) { + if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD); } if (axis == YAW) { - if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = 0; + if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW); } if (antiWindupProtection || motorLimitReached) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 2c190feb9..8920d0b82 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -25,6 +25,8 @@ #define PID_LAST_RATE_COUNT 7 #define PID_DTERM_FIR_MAX_LENGTH 7 #define PID_MAX_DIFFERENTIATOR (PID_DTERM_FIR_MAX_LENGTH-2) +#define ITERM_RESET_THRESHOLD 20 +#define ITERM_RESET_THRESHOLD_YAW 10 typedef enum { PIDROLL, @@ -57,6 +59,12 @@ typedef enum { RESET_ITERM_AND_REDUCE_PID } pidErrorResetOption_e; +typedef enum { + SUPEREXPO_YAW_OFF = 0, + SUPEREXPO_YAW_ON, + SUPEREXPO_YAW_ALWAYS +} pidSuperExpoYaw_e; + #define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2) typedef struct pidProfile_s { @@ -70,8 +78,9 @@ typedef struct pidProfile_s { float dterm_lpf_hz; // Delta Filter in hz float yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy - uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates - uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates + uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates + uint8_t rollPitchItermResetAlways; // Reset Iterm also without SUPER EXPO + uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates uint8_t deltaMethod; // Alternative delta Calculation uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 550293fee..a218243f3 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -440,6 +440,10 @@ static const char * const lookupTableDebug[] = { "AIRMODE" }; +static const char * const lookupTableSuperExpoYaw[] = { + "OFF", "ON", "ALWAYS" +}; + typedef struct lookupTableEntry_s { const char * const *values; const uint8_t valueCount; @@ -466,6 +470,7 @@ typedef enum { TABLE_MAG_HARDWARE, TABLE_DELTA_METHOD, TABLE_DEBUG, + TABLE_SUPEREXPO_YAW, } lookupTableIndex_e; static const lookupTableEntry_t lookupTables[] = { @@ -488,7 +493,8 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) }, { lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) }, { lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) }, - { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) } + { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }, + { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) } }; #define VALUE_TYPE_OFFSET 0 @@ -688,6 +694,8 @@ const clivalue_t valueTable[] = { { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, { "super_expo_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactor, .config.minmax = {1, 100 } }, + { "super_expo_factor_yaw", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactorYaw, .config.minmax = {1, 100 } }, + { "super_expo_yaw", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.superExpoYawMode, .config.lookup = { TABLE_SUPEREXPO_YAW } }, { "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0, 200 } }, { "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_off_delay, .config.minmax = { 0, 200 } }, @@ -722,6 +730,7 @@ const clivalue_t valueTable[] = { { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_differentiator", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_differentiator, .config.lookup = { TABLE_OFF_ON } }, + { "iterm_always_reset", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } }, { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, { "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 121f0d636..6e2dd2124 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -125,6 +125,8 @@ typedef struct rxConfig_s { uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands uint8_t max_aux_channel; uint8_t superExpoFactor; // Super Expo Factor + uint8_t superExpoFactorYaw; // Super Expo Factor Yaw + uint8_t superExpoYawMode; // Seperate Super expo for yaw uint16_t rx_min_usec; uint16_t rx_max_usec; From bef46dd120e1a286079bf5d6ca72f3f5edd69449 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 27 Apr 2016 22:15:39 +0200 Subject: [PATCH 68/78] TPA added for Yaw // Cleanup unused config params --- src/main/blackbox/blackbox.c | 4 ---- src/main/config/config.c | 1 - src/main/flight/pid.h | 3 --- src/main/io/serial_cli.c | 7 ------- src/main/mw.c | 9 ++------- 5 files changed, 2 insertions(+), 22 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b0413f6bf..0190affa1 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1295,10 +1295,6 @@ static bool blackboxWriteSysinfo() blackboxPrintfHeaderLine("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0)); break; - case 40: - blackboxPrintfHeaderLine("deltaMethod:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.deltaMethod); - break; case 41: blackboxPrintfHeaderLine("H_sensitivity:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.H_sensitivity); diff --git a/src/main/config/config.c b/src/main/config/config.c index 822e95515..bb91db4d2 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -183,7 +183,6 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->rollPitchItermResetAlways = 0; pidProfile->yawItermResetRate = 50; pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default - pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->H_sensitivity = 75; // TODO - Cleanup during next EEPROM changes diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 8920d0b82..94addfb55 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -23,8 +23,6 @@ #define YAW_P_LIMIT_MAX 400 // Maximum value for yaw P limiter #define PID_LAST_RATE_COUNT 7 -#define PID_DTERM_FIR_MAX_LENGTH 7 -#define PID_MAX_DIFFERENTIATOR (PID_DTERM_FIR_MAX_LENGTH-2) #define ITERM_RESET_THRESHOLD 20 #define ITERM_RESET_THRESHOLD_YAW 10 @@ -81,7 +79,6 @@ typedef struct pidProfile_s { uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates uint8_t rollPitchItermResetAlways; // Reset Iterm also without SUPER EXPO uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates - uint8_t deltaMethod; // Alternative delta Calculation uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm uint8_t dterm_differentiator; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a218243f3..2f837ed9e 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -426,10 +426,6 @@ static const char * const lookupTableMagHardware[] = { "AK8963" }; -static const char * const lookupDeltaMethod[] = { - "ERROR", "MEASUREMENT" -}; - static const char * const lookupTableDebug[] = { "NONE", "CYCLETIME", @@ -468,7 +464,6 @@ typedef enum { TABLE_ACC_HARDWARE, TABLE_BARO_HARDWARE, TABLE_MAG_HARDWARE, - TABLE_DELTA_METHOD, TABLE_DEBUG, TABLE_SUPEREXPO_YAW, } lookupTableIndex_e; @@ -492,7 +487,6 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) }, { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) }, { lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) }, - { lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) }, { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }, { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) } }; @@ -727,7 +721,6 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, - { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_differentiator", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_differentiator, .config.lookup = { TABLE_OFF_ON } }, { "iterm_always_reset", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } }, diff --git a/src/main/mw.c b/src/main/mw.c index 7f7497f6b..a7b184758 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -261,13 +261,8 @@ void annexCode(void) rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction; } - // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids - if (axis == YAW) { - PIDweight[axis] = 100; - } - else { - PIDweight[axis] = prop; - } + // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. + PIDweight[axis] = prop; if (rcData[axis] < masterConfig.rxConfig.midrc) rcCommand[axis] = -rcCommand[axis]; From 1031e40a520dbc48587be5196cc8871a6b340053 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 27 Apr 2016 22:51:49 +0200 Subject: [PATCH 69/78] Change min_check default to something higher // Prevent arming issus on defaults --- src/main/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index bb91db4d2..116692ee1 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -435,7 +435,7 @@ static void resetConf(void) masterConfig.rxConfig.spektrum_sat_bind = 0; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; masterConfig.rxConfig.midrc = 1500; - masterConfig.rxConfig.mincheck = 1040; + masterConfig.rxConfig.mincheck = 1080; masterConfig.rxConfig.maxcheck = 1900; masterConfig.rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection From f27932e236dc236cd2438cf68a116f84a9fd666a Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Thu, 28 Apr 2016 06:35:46 +0100 Subject: [PATCH 70/78] Renumber Header Indexes Renumbered the header index after removal of Item 40 on. --- src/main/blackbox/blackbox.c | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 0190affa1..548a62856 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1295,44 +1295,44 @@ static bool blackboxWriteSysinfo() blackboxPrintfHeaderLine("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0)); break; - case 41: + case 40: blackboxPrintfHeaderLine("H_sensitivity:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.H_sensitivity); break; - case 42: + case 41: blackboxPrintfHeaderLine("deadband:%d", masterConfig.rcControlsConfig.deadband); break; - case 43: + case 42: blackboxPrintfHeaderLine("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); break; - case 44: + case 43: blackboxPrintfHeaderLine("gyro_lpf:%d", masterConfig.gyro_lpf); break; - case 45: + case 44: blackboxPrintfHeaderLine("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0)); break; - case 46: + case 45: blackboxPrintfHeaderLine("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0)); break; - case 47: + case 46: blackboxPrintfHeaderLine("acc_hardware:%d", masterConfig.acc_hardware); break; - case 48: + case 47: blackboxPrintfHeaderLine("baro_hardware:%d", masterConfig.baro_hardware); break; - case 49: + case 48: blackboxPrintfHeaderLine("mag_hardware:%d", masterConfig.mag_hardware); break; - case 50: + case 49: blackboxPrintfHeaderLine("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm); break; - case 51: + case 50: blackboxPrintfHeaderLine("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation); break; - case 52: + case 51: blackboxPrintfHeaderLine("rc_smoothing:%d", masterConfig.rxConfig.rcSmoothing); break; - case 53: + case 52: blackboxPrintfHeaderLine("features:%d", masterConfig.enabledFeatures); break; default: From 7af7ddcac0c74dc7ddd0b1951b001ba959f25d38 Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Thu, 28 Apr 2016 17:10:23 +0100 Subject: [PATCH 71/78] Add Betaflight Revision to Header Field Added Betaflight version number to header; no more doubt about which version you were flying when the log was made. --- src/main/blackbox/blackbox.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 548a62856..e8306a4c4 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1133,7 +1133,7 @@ static bool blackboxWriteSysinfo() blackboxPrintfHeaderLine("Firmware type:Cleanflight"); break; case 1: - blackboxPrintfHeaderLine("Firmware revision:%s", shortGitRevision); + blackboxPrintfHeaderLine("Firmware revision:Betaflight %s (%s)", FC_VERSION_STRING, shortGitRevision); break; case 2: blackboxPrintfHeaderLine("Firmware date:%s %s", buildDate, buildTime); From a36fe5099f43867f3bd1d1dd6a023dfabc363297 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 30 Apr 2016 09:57:25 +0200 Subject: [PATCH 72/78] Fix float / double warnings --- src/main/blackbox/blackbox.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index e8306a4c4..e73f69387 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1273,7 +1273,7 @@ static bool blackboxWriteSysinfo() break; case 34: blackboxPrintfHeaderLine("yaw_lpf_hz:%d", - (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0)); + (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f)); break; case 35: blackboxPrintfHeaderLine("dterm_average_count:%d", @@ -1293,7 +1293,7 @@ static bool blackboxWriteSysinfo() break; case 39: blackboxPrintfHeaderLine("dterm_lpf_hz:%d", - (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0)); + (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f)); break; case 40: blackboxPrintfHeaderLine("H_sensitivity:%d", @@ -1309,10 +1309,10 @@ static bool blackboxWriteSysinfo() blackboxPrintfHeaderLine("gyro_lpf:%d", masterConfig.gyro_lpf); break; case 44: - blackboxPrintfHeaderLine("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0)); + blackboxPrintfHeaderLine("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f)); break; case 45: - blackboxPrintfHeaderLine("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0)); + blackboxPrintfHeaderLine("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); break; case 46: blackboxPrintfHeaderLine("acc_hardware:%d", masterConfig.acc_hardware); From d62d894cc86036c7c1ced73aa875a0a94a497ca7 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 30 Apr 2016 10:52:48 +0200 Subject: [PATCH 73/78] Cleanup unused variables Fix --- src/main/mw.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/mw.c b/src/main/mw.c index a7b184758..80d588baf 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -112,7 +112,7 @@ int16_t telemTemperature1; // gyro sensor temperature static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero extern uint32_t currentTime; -extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; +extern uint8_t PIDweight[3]; extern bool antiWindupProtection; uint16_t filteredCycleTime; From 46a49f61470e4a34adc5998ae05fc966b4d65d41 Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 30 Apr 2016 12:16:02 -0700 Subject: [PATCH 74/78] hardfault handler w/ debugging info and an automatic breakpoint --- Makefile | 17 +- src/main/main.c | 61 +++ .../startup_stm32f3_debug_hardfault_handler.S | 501 ++++++++++++++++++ 3 files changed, 576 insertions(+), 3 deletions(-) create mode 100644 src/main/startup/startup_stm32f3_debug_hardfault_handler.S diff --git a/Makefile b/Makefile index 90217b59a..723545eee 100644 --- a/Makefile +++ b/Makefile @@ -28,6 +28,10 @@ OPBL ?=no # Debugger optons, must be empty or GDB DEBUG ?= +# Insert the debugging hardfault debugger +# releases should not be built with this flag as it does not disable pwm output +DEBUG_HARDFAULTS ?= + # Serial port/Device for flashing SERIAL_DEVICE ?= $(firstword $(wildcard /dev/ttyUSB*) no-port-found) @@ -56,6 +60,13 @@ OPBL_VALID_TARGETS = CC3D_OPBL F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI SPRACINGF3EVO DOGE +# note that there is no hardfault debugging startup file assembly handler for other platforms +ifeq ($(DEBUG_HARDFAULTS),F3) +CFLAGS += -DDEBUG_HARDFAULTS +STM32F30x_COMMON_SRC = startup_stm32f3_debug_hardfault_handler.S +else +STM32F30x_COMMON_SRC = startup_stm32f30x_md_gcc.S +endif # Configure default flash sizes for the targets ifeq ($(FLASH_SIZE),) @@ -534,8 +545,8 @@ CC3D_SRC = \ $(COMMON_SRC) \ $(VCP_SRC) -STM32F30x_COMMON_SRC = \ - startup_stm32f30x_md_gcc.S \ + +STM32F30x_COMMON_SRC += \ drivers/adc.c \ drivers/adc_stm32f30x.c \ drivers/bus_i2c_stm32f30x.c \ @@ -816,7 +827,7 @@ endif DEBUG_FLAGS = -ggdb3 -DDEBUG -CFLAGS = $(ARCH_FLAGS) \ +CFLAGS += $(ARCH_FLAGS) \ $(LTO_FLAGS) \ $(addprefix -D,$(OPTIONS)) \ $(addprefix -I,$(INCLUDE_DIRS)) \ diff --git a/src/main/main.c b/src/main/main.c index 59158b2a5..49fc7c246 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -736,6 +736,66 @@ int main(void) { } } +#ifdef DEBUG_HARDFAULTS +//from: https://mcuoneclipse.com/2012/11/24/debugging-hard-faults-on-arm-cortex-m/ +/** + * hard_fault_handler_c: + * This is called from the HardFault_HandlerAsm with a pointer the Fault stack + * as the parameter. We can then read the values from the stack and place them + * into local variables for ease of reading. + * We then read the various Fault Status and Address Registers to help decode + * cause of the fault. + * The function ends with a BKPT instruction to force control back into the debugger + */ +void hard_fault_handler_c(unsigned long *hardfault_args){ + volatile unsigned long stacked_r0 ; + volatile unsigned long stacked_r1 ; + volatile unsigned long stacked_r2 ; + volatile unsigned long stacked_r3 ; + volatile unsigned long stacked_r12 ; + volatile unsigned long stacked_lr ; + volatile unsigned long stacked_pc ; + volatile unsigned long stacked_psr ; + volatile unsigned long _CFSR ; + volatile unsigned long _HFSR ; + volatile unsigned long _DFSR ; + volatile unsigned long _AFSR ; + volatile unsigned long _BFAR ; + volatile unsigned long _MMAR ; + + stacked_r0 = ((unsigned long)hardfault_args[0]) ; + stacked_r1 = ((unsigned long)hardfault_args[1]) ; + stacked_r2 = ((unsigned long)hardfault_args[2]) ; + stacked_r3 = ((unsigned long)hardfault_args[3]) ; + stacked_r12 = ((unsigned long)hardfault_args[4]) ; + stacked_lr = ((unsigned long)hardfault_args[5]) ; + stacked_pc = ((unsigned long)hardfault_args[6]) ; + stacked_psr = ((unsigned long)hardfault_args[7]) ; + + // Configurable Fault Status Register + // Consists of MMSR, BFSR and UFSR + _CFSR = (*((volatile unsigned long *)(0xE000ED28))) ; + + // Hard Fault Status Register + _HFSR = (*((volatile unsigned long *)(0xE000ED2C))) ; + + // Debug Fault Status Register + _DFSR = (*((volatile unsigned long *)(0xE000ED30))) ; + + // Auxiliary Fault Status Register + _AFSR = (*((volatile unsigned long *)(0xE000ED3C))) ; + + // Read the Fault Address Registers. These may not contain valid values. + // Check BFARVALID/MMARVALID to see if they are valid values + // MemManage Fault Address Register + _MMAR = (*((volatile unsigned long *)(0xE000ED34))) ; + // Bus Fault Address Register + _BFAR = (*((volatile unsigned long *)(0xE000ED38))) ; + + __asm("BKPT #0\n") ; // Break into the debugger +} + +#else void HardFault_Handler(void) { // fall out of the sky @@ -753,3 +813,4 @@ void HardFault_Handler(void) while (1); } +#endif diff --git a/src/main/startup/startup_stm32f3_debug_hardfault_handler.S b/src/main/startup/startup_stm32f3_debug_hardfault_handler.S new file mode 100644 index 000000000..65ee39a67 --- /dev/null +++ b/src/main/startup/startup_stm32f3_debug_hardfault_handler.S @@ -0,0 +1,501 @@ +/** + ****************************************************************************** + * @file startup_stm32f30x.s + * @author MCD Application Team + * @version V1.0.0 + * @date 04-Spetember-2012 + * @brief STM32F30x Devices vector table for RIDE7 toolchain. + * This module performs: + * - Set the initial SP + * - Set the initial PC == Reset_Handler, + * - Set the vector table entries with the exceptions ISR address + * - Configure the clock system and the external SRAM mounted on + * STM3230C-EVAL board to be used as data memory (optional, + * to be enabled by user) + * - Branches to main in the C library (which eventually + * calls main()). + * After Reset the Cortex-M4 processor is in Thread mode, + * priority is Privileged, and the Stack is set to Main. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + .syntax unified + .cpu cortex-m4 + .fpu softvfp + .thumb + +.global g_pfnVectors +.global Default_Handler + +.global HardFault_Handler +.extern hard_fault_handler_c + +/* start address for the initialization values of the .data section. +defined in linker script */ +.word _sidata +/* start address for the .data section. defined in linker script */ +.word _sdata +/* end address for the .data section. defined in linker script */ +.word _edata +/* start address for the .bss section. defined in linker script */ +.word _sbss +/* end address for the .bss section. defined in linker script */ +.word _ebss +/* stack used for SystemInit_ExtMemCtl; always internal RAM used */ + +/** + * @brief This is the code that gets called when the processor first + * starts execution following a reset event. Only the absolutely + * necessary set is performed, after which the application + * supplied main() routine is called. + * @param None + * @retval : None +*/ + + .section .text.Reset_Handler + .weak Reset_Handler + .type Reset_Handler, %function +Reset_Handler: + ldr r0, =0x20009FFC // HJI 11/9/2012 + ldr r1, =0xDEADBEEF // HJI 11/9/2012 + ldr r2, [r0, #0] // HJI 11/9/2012 + str r0, [r0, #0] // HJI 11/9/2012 + cmp r2, r1 // HJI 11/9/2012 + beq Reboot_Loader // HJI 11/9/2012 + +/* Copy the data segment initializers from flash to SRAM */ + movs r1, #0 + b LoopCopyDataInit + +CopyDataInit: + ldr r3, =_sidata + ldr r3, [r3, r1] + str r3, [r0, r1] + adds r1, r1, #4 + +LoopCopyDataInit: + ldr r0, =_sdata + ldr r3, =_edata + adds r2, r0, r1 + cmp r2, r3 + bcc CopyDataInit + ldr r2, =_sbss + b LoopFillZerobss +/* Zero fill the bss segment. */ +FillZerobss: + movs r3, #0 + str r3, [r2], #4 + +LoopFillZerobss: + ldr r3, = _ebss + cmp r2, r3 + bcc FillZerobss + +/* Call the clock system intitialization function.*/ + bl SystemInit +/* Call the application's entry point.*/ + bl main + bx lr + +LoopForever: + b LoopForever + +Reboot_Loader: // HJI 11/9/2012 + + // Reboot to ROM // HJI 11/9/2012 + ldr r0, =0x1FFFD800 // HJI 4/26/2013 + ldr sp,[r0, #0] // HJI 11/9/2012 + ldr r0,[r0, #4] // HJI 11/9/2012 + bx r0 // HJI 11/9/2012 + +.size Reset_Handler, .-Reset_Handler + +.section .text.Reset_Handler +.weak HardFault_Handler +.type HardFault_Handler, %function +HardFault_Handler: + movs r0,#4 + movs r1, lr + tst r0, r1 + beq _MSP + mrs r0, psp + b _HALT +_MSP: + mrs r0, msp +_HALT: + ldr r1,[r0,#20] + b hard_fault_handler_c + bkpt #0 + +.size HardFault_Handler, .-HardFault_Handler + +/** + * @brief This is the code that gets called when the processor receives an + * unexpected interrupt. This simply enters an infinite loop, preserving + * the system state for examination by a debugger. + * @param None + * @retval None +*/ + .section .text.Default_Handler,"ax",%progbits +Default_Handler: +Infinite_Loop: + b Infinite_Loop + .size Default_Handler, .-Default_Handler +/****************************************************************************** +* +* The minimal vector table for a Cortex M4. Note that the proper constructs +* must be placed on this to ensure that it ends up at physical address +* 0x0000.0000. +* +*******************************************************************************/ + .section .isr_vector,"a",%progbits + .type g_pfnVectors, %object + .size g_pfnVectors, .-g_pfnVectors + +g_pfnVectors: + .word _estack + .word Reset_Handler + .word NMI_Handler + .word HardFault_Handler + .word MemManage_Handler + .word BusFault_Handler + .word UsageFault_Handler + .word 0 + .word 0 + .word 0 + .word 0 + .word SVC_Handler + .word DebugMon_Handler + .word 0 + .word PendSV_Handler + .word SysTick_Handler + .word WWDG_IRQHandler + .word PVD_IRQHandler + .word TAMPER_STAMP_IRQHandler + .word RTC_WKUP_IRQHandler + .word FLASH_IRQHandler + .word RCC_IRQHandler + .word EXTI0_IRQHandler + .word EXTI1_IRQHandler + .word EXTI2_TS_IRQHandler + .word EXTI3_IRQHandler + .word EXTI4_IRQHandler + .word DMA1_Channel1_IRQHandler + .word DMA1_Channel2_IRQHandler + .word DMA1_Channel3_IRQHandler + .word DMA1_Channel4_IRQHandler + .word DMA1_Channel5_IRQHandler + .word DMA1_Channel6_IRQHandler + .word DMA1_Channel7_IRQHandler + .word ADC1_2_IRQHandler + .word USB_HP_CAN1_TX_IRQHandler + .word USB_LP_CAN1_RX0_IRQHandler + .word CAN1_RX1_IRQHandler + .word CAN1_SCE_IRQHandler + .word EXTI9_5_IRQHandler + .word TIM1_BRK_TIM15_IRQHandler + .word TIM1_UP_TIM16_IRQHandler + .word TIM1_TRG_COM_TIM17_IRQHandler + .word TIM1_CC_IRQHandler + .word TIM2_IRQHandler + .word TIM3_IRQHandler + .word TIM4_IRQHandler + .word I2C1_EV_IRQHandler + .word I2C1_ER_IRQHandler + .word I2C2_EV_IRQHandler + .word I2C2_ER_IRQHandler + .word SPI1_IRQHandler + .word SPI2_IRQHandler + .word USART1_IRQHandler + .word USART2_IRQHandler + .word USART3_IRQHandler + .word EXTI15_10_IRQHandler + .word RTC_Alarm_IRQHandler + .word USBWakeUp_IRQHandler + .word TIM8_BRK_IRQHandler + .word TIM8_UP_IRQHandler + .word TIM8_TRG_COM_IRQHandler + .word TIM8_CC_IRQHandler + .word ADC3_IRQHandler + .word 0 + .word 0 + .word 0 + .word SPI3_IRQHandler + .word UART4_IRQHandler + .word UART5_IRQHandler + .word TIM6_DAC_IRQHandler + .word TIM7_IRQHandler + .word DMA2_Channel1_IRQHandler + .word DMA2_Channel2_IRQHandler + .word DMA2_Channel3_IRQHandler + .word DMA2_Channel4_IRQHandler + .word DMA2_Channel5_IRQHandler + .word ADC4_IRQHandler + .word 0 + .word 0 + .word COMP1_2_3_IRQHandler + .word COMP4_5_6_IRQHandler + .word COMP7_IRQHandler + .word 0 + .word 0 + .word 0 + .word 0 + .word 0 + .word 0 + .word 0 + .word USB_HP_IRQHandler + .word USB_LP_IRQHandler + .word USBWakeUp_RMP_IRQHandler + .word 0 + .word 0 + .word 0 + .word 0 + .word FPU_IRQHandler + +/******************************************************************************* +* +* Provide weak aliases for each Exception handler to the Default_Handler. +* As they are weak aliases, any function with the same name will override +* this definition. +* +*******************************************************************************/ + + .weak NMI_Handler + .thumb_set NMI_Handler,Default_Handler + + .weak MemManage_Handler + .thumb_set MemManage_Handler,Default_Handler + + .weak BusFault_Handler + .thumb_set BusFault_Handler,Default_Handler + + .weak UsageFault_Handler + .thumb_set UsageFault_Handler,Default_Handler + + .weak SVC_Handler + .thumb_set SVC_Handler,Default_Handler + + .weak DebugMon_Handler + .thumb_set DebugMon_Handler,Default_Handler + + .weak PendSV_Handler + .thumb_set PendSV_Handler,Default_Handler + + .weak SysTick_Handler + .thumb_set SysTick_Handler,Default_Handler + + .weak WWDG_IRQHandler + .thumb_set WWDG_IRQHandler,Default_Handler + + .weak PVD_IRQHandler + .thumb_set PVD_IRQHandler,Default_Handler + + .weak TAMPER_STAMP_IRQHandler + .thumb_set TAMPER_STAMP_IRQHandler,Default_Handler + + .weak RTC_WKUP_IRQHandler + .thumb_set RTC_WKUP_IRQHandler,Default_Handler + + .weak FLASH_IRQHandler + .thumb_set FLASH_IRQHandler,Default_Handler + + .weak RCC_IRQHandler + .thumb_set RCC_IRQHandler,Default_Handler + + .weak EXTI0_IRQHandler + .thumb_set EXTI0_IRQHandler,Default_Handler + + .weak EXTI1_IRQHandler + .thumb_set EXTI1_IRQHandler,Default_Handler + + .weak EXTI2_TS_IRQHandler + .thumb_set EXTI2_TS_IRQHandler,Default_Handler + + .weak EXTI3_IRQHandler + .thumb_set EXTI3_IRQHandler,Default_Handler + + .weak EXTI4_IRQHandler + .thumb_set EXTI4_IRQHandler,Default_Handler + + .weak DMA1_Channel1_IRQHandler + .thumb_set DMA1_Channel1_IRQHandler,Default_Handler + + .weak DMA1_Channel2_IRQHandler + .thumb_set DMA1_Channel2_IRQHandler,Default_Handler + + .weak DMA1_Channel3_IRQHandler + .thumb_set DMA1_Channel3_IRQHandler,Default_Handler + + .weak DMA1_Channel4_IRQHandler + .thumb_set DMA1_Channel4_IRQHandler,Default_Handler + + .weak DMA1_Channel5_IRQHandler + .thumb_set DMA1_Channel5_IRQHandler,Default_Handler + + .weak DMA1_Channel6_IRQHandler + .thumb_set DMA1_Channel6_IRQHandler,Default_Handler + + .weak DMA1_Channel7_IRQHandler + .thumb_set DMA1_Channel7_IRQHandler,Default_Handler + + .weak ADC1_2_IRQHandler + .thumb_set ADC1_2_IRQHandler,Default_Handler + + .weak USB_HP_CAN1_TX_IRQHandler + .thumb_set USB_HP_CAN1_TX_IRQHandler,Default_Handler + + .weak USB_LP_CAN1_RX0_IRQHandler + .thumb_set USB_LP_CAN1_RX0_IRQHandler,Default_Handler + + .weak CAN1_RX1_IRQHandler + .thumb_set CAN1_RX1_IRQHandler,Default_Handler + + .weak CAN1_SCE_IRQHandler + .thumb_set CAN1_SCE_IRQHandler,Default_Handler + + .weak EXTI9_5_IRQHandler + .thumb_set EXTI9_5_IRQHandler,Default_Handler + + .weak TIM1_BRK_TIM15_IRQHandler + .thumb_set TIM1_BRK_TIM15_IRQHandler,Default_Handler + + .weak TIM1_UP_TIM16_IRQHandler + .thumb_set TIM1_UP_TIM16_IRQHandler,Default_Handler + + .weak TIM1_TRG_COM_TIM17_IRQHandler + .thumb_set TIM1_TRG_COM_TIM17_IRQHandler,Default_Handler + + .weak TIM1_CC_IRQHandler + .thumb_set TIM1_CC_IRQHandler,Default_Handler + + .weak TIM2_IRQHandler + .thumb_set TIM2_IRQHandler,Default_Handler + + .weak TIM3_IRQHandler + .thumb_set TIM3_IRQHandler,Default_Handler + + .weak TIM4_IRQHandler + .thumb_set TIM4_IRQHandler,Default_Handler + + .weak I2C1_EV_IRQHandler + .thumb_set I2C1_EV_IRQHandler,Default_Handler + + .weak I2C1_ER_IRQHandler + .thumb_set I2C1_ER_IRQHandler,Default_Handler + + .weak I2C2_EV_IRQHandler + .thumb_set I2C2_EV_IRQHandler,Default_Handler + + .weak I2C2_ER_IRQHandler + .thumb_set I2C2_ER_IRQHandler,Default_Handler + + .weak SPI1_IRQHandler + .thumb_set SPI1_IRQHandler,Default_Handler + + .weak SPI2_IRQHandler + .thumb_set SPI2_IRQHandler,Default_Handler + + .weak USART1_IRQHandler + .thumb_set USART1_IRQHandler,Default_Handler + + .weak USART2_IRQHandler + .thumb_set USART2_IRQHandler,Default_Handler + + .weak USART3_IRQHandler + .thumb_set USART3_IRQHandler,Default_Handler + + .weak EXTI15_10_IRQHandler + .thumb_set EXTI15_10_IRQHandler,Default_Handler + + .weak RTC_Alarm_IRQHandler + .thumb_set RTC_Alarm_IRQHandler,Default_Handler + + .weak USBWakeUp_IRQHandler + .thumb_set USBWakeUp_IRQHandler,Default_Handler + + .weak TIM8_BRK_IRQHandler + .thumb_set TIM8_BRK_IRQHandler,Default_Handler + + .weak TIM8_UP_IRQHandler + .thumb_set TIM8_UP_IRQHandler,Default_Handler + + .weak TIM8_TRG_COM_IRQHandler + .thumb_set TIM8_TRG_COM_IRQHandler,Default_Handler + + .weak TIM8_CC_IRQHandler + .thumb_set TIM8_CC_IRQHandler,Default_Handler + + .weak ADC3_IRQHandler + .thumb_set ADC3_IRQHandler,Default_Handler + + .weak SPI3_IRQHandler + .thumb_set SPI3_IRQHandler,Default_Handler + + .weak UART4_IRQHandler + .thumb_set UART4_IRQHandler,Default_Handler + + .weak UART5_IRQHandler + .thumb_set UART5_IRQHandler,Default_Handler + + .weak TIM6_DAC_IRQHandler + .thumb_set TIM6_DAC_IRQHandler,Default_Handler + + .weak TIM7_IRQHandler + .thumb_set TIM7_IRQHandler,Default_Handler + + .weak DMA2_Channel1_IRQHandler + .thumb_set DMA2_Channel1_IRQHandler,Default_Handler + + .weak DMA2_Channel2_IRQHandler + .thumb_set DMA2_Channel2_IRQHandler,Default_Handler + + .weak DMA2_Channel3_IRQHandler + .thumb_set DMA2_Channel3_IRQHandler,Default_Handler + + .weak DMA2_Channel4_IRQHandler + .thumb_set DMA2_Channel4_IRQHandler,Default_Handler + + .weak DMA2_Channel5_IRQHandler + .thumb_set DMA2_Channel5_IRQHandler,Default_Handler + + .weak ADC4_IRQHandler + .thumb_set ADC4_IRQHandler,Default_Handler + + .weak COMP1_2_3_IRQHandler + .thumb_set COMP1_2_3_IRQHandler,Default_Handler + + .weak COMP4_5_6_IRQHandler + .thumb_set COMP4_5_6_IRQHandler,Default_Handler + + .weak COMP7_IRQHandler + .thumb_set COMP7_IRQHandler,Default_Handler + + .weak USB_HP_IRQHandler + .thumb_set USB_HP_IRQHandler,Default_Handler + + .weak USB_LP_IRQHandler + .thumb_set USB_LP_IRQHandler,Default_Handler + + .weak USBWakeUp_RMP_IRQHandler + .thumb_set USBWakeUp_RMP_IRQHandler,Default_Handler + + .weak FPU_IRQHandler + .thumb_set FPU_IRQHandler,Default_Handler +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ From 96f4d090a2891d225a53cdcbab0947e590673322 Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 30 Apr 2016 12:34:55 -0700 Subject: [PATCH 75/78] dont try to check the gyro status if the gyro doesnt support interrupts --- src/main/drivers/gyro_sync.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index 1047442c0..934708f37 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -32,7 +32,8 @@ static uint8_t mpuDividerDrops; bool getMpuDataStatus(gyro_t *gyro) { bool mpuDataStatus; - + if (!gyro->intStatus) + return false; gyro->intStatus(&mpuDataStatus); return mpuDataStatus; } From c89926e9cc7c3314195188b88b423760b5c1a5a9 Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 30 Apr 2016 12:35:09 -0700 Subject: [PATCH 76/78] duplicate file --- Makefile | 1 - 1 file changed, 1 deletion(-) diff --git a/Makefile b/Makefile index 723545eee..d5bfb1d00 100644 --- a/Makefile +++ b/Makefile @@ -575,7 +575,6 @@ STM32F3DISCOVERY_COMMON_SRC = \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stm32f30x.c \ drivers/accgyro_l3gd20.c \ - drivers/accgyro_l3gd20.c \ drivers/accgyro_lsm303dlhc.c \ drivers/compass_hmc5883l.c \ $(VCP_SRC) From 2aea052f5207a1c003db6ecd8bcccebce5bc29bf Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 30 Apr 2016 12:41:43 -0700 Subject: [PATCH 77/78] print the target name so we can figure out which target is breaking w/ fake_travis_build.sh --- fake_travis_build.sh | 3 +++ 1 file changed, 3 insertions(+) diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 2a6a9b275..9d1fe6616 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -26,6 +26,9 @@ export TRAVIS_REPO_SLUG=${TRAVIS_REPO_SLUG:=$USER/simulated} for target in "${targets[@]}" do unset RUNTESTS PUBLISHMETA TARGET + echo + echo + echo "BUILDING '$target'" eval "export $target" make -f Makefile clean ./.travis.sh From 79d5414f3e99030dcd33a0b97ac482e72b5a36fa Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 30 Apr 2016 14:33:36 -0700 Subject: [PATCH 78/78] guess we need some defines --- src/main/target/STM32F3DISCOVERY/target.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 8a812d879..190676ef3 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -152,6 +152,20 @@ #define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3 #define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 +#define LED_STRIP +#define LED_STRIP_TIMER TIM16 +#define WS2811_GPIO GPIOB +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB +#define WS2811_GPIO_AF GPIO_AF_1 +#define WS2811_PIN GPIO_Pin_8 // TIM16_CH1 +#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_TIMER TIM16 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 +#define WS2811_DMA_CHANNEL DMA1_Channel3 +#define WS2811_IRQ DMA1_Channel3_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER + #define BLACKBOX #define GPS //#define GTUNE