From 6bd1cc4deac845a6b397a7c5b82124c7523c9543 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 23 Jun 2016 09:56:41 +0100 Subject: [PATCH] BST tidy --- src/main/drivers/nvic.h | 1 - src/main/target/COLIBRI_RACE/bus_bst.h | 31 +- .../target/COLIBRI_RACE/bus_bst_stm32f30x.c | 504 ++-- src/main/target/COLIBRI_RACE/i2c_bst.c | 2066 ++++++++--------- 4 files changed, 1300 insertions(+), 1302 deletions(-) diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h index 7d6b100a9..7c4b9c7cd 100644 --- a/src/main/drivers/nvic.h +++ b/src/main/drivers/nvic.h @@ -38,7 +38,6 @@ #define NVIC_PRIO_MPU_DATA_READY NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_MAG_DATA_READY NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_CALLBACK NVIC_BUILD_PRIORITY(0x0f, 0x0f) -#define NVIC_PRIO_BST_READ_DATA NVIC_BUILD_PRIORITY(1, 1) // utility macros to join/split priority #define NVIC_BUILD_PRIORITY(base,sub) (((((base)<<(4-(7-(NVIC_PRIORITY_GROUPING>>8))))|((sub)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8)))))<<4)&0xf0) diff --git a/src/main/target/COLIBRI_RACE/bus_bst.h b/src/main/target/COLIBRI_RACE/bus_bst.h index 3ade4baee..162fd72b1 100644 --- a/src/main/target/COLIBRI_RACE/bus_bst.h +++ b/src/main/target/COLIBRI_RACE/bus_bst.h @@ -17,22 +17,22 @@ #pragma once -#define TBS_CORE_PNP_PRO 0x80 -#define RESERVED 0x8A -#define PNP_PRO_DIDITAL_CURRENT_SENSOR 0xC0 -#define PNP_PRO_GPS 0xC2 -#define TSB_BLACKBOX 0xC4 -#define CLEANFLIGHT_FC 0xC8 -#define CROSSFIRE_UHF_RECEIVER 0xEC +#define TBS_CORE_PNP_PRO 0x80 +#define RESERVED 0x8A +#define PNP_PRO_DIDITAL_CURRENT_SENSOR 0xC0 +#define PNP_PRO_GPS 0xC2 +#define TSB_BLACKBOX 0xC4 +#define CLEANFLIGHT_FC 0xC8 +#define CROSSFIRE_UHF_RECEIVER 0xEC -#define GPS_POSITION_FRAME_ID 0x02 -#define GPS_TIME_FRAME_ID 0x03 -#define FC_ATTITUDE_FRAME_ID 0x1E -#define RC_CHANNEL_FRAME_ID 0x15 -#define CROSSFIRE_RSSI_FRAME_ID 0x14 -#define CLEANFLIGHT_MODE_FRAME_ID 0x20 +#define GPS_POSITION_FRAME_ID 0x02 +#define GPS_TIME_FRAME_ID 0x03 +#define FC_ATTITUDE_FRAME_ID 0x1E +#define RC_CHANNEL_FRAME_ID 0x15 +#define CROSSFIRE_RSSI_FRAME_ID 0x14 +#define CLEANFLIGHT_MODE_FRAME_ID 0x20 -#define DATA_BUFFER_SIZE 64 +#define DATA_BUFFER_SIZE 64 typedef enum BSTDevice { BSTDEV_1, @@ -53,6 +53,3 @@ void bstMasterWriteLoop(void); void crc8Cal(uint8_t data_in); - - - diff --git a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c index b9007dd84..f23932a6e 100644 --- a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c +++ b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c @@ -17,33 +17,35 @@ #ifdef USE_BST -#define BST_SHORT_TIMEOUT ((uint32_t)0x1000) -#define BST_LONG_TIMEOUT ((uint32_t)(10 * BST_SHORT_TIMEOUT)) +#define NVIC_PRIO_BST_READ_DATA NVIC_BUILD_PRIORITY(1, 1) + +#define BST_SHORT_TIMEOUT ((uint32_t)0x1000) +#define BST_LONG_TIMEOUT ((uint32_t)(10 * BST_SHORT_TIMEOUT)) #if !defined(BST1_SCL_GPIO) -#define BST1_SCL_GPIO GPIOB -#define BST1_SCL_GPIO_AF GPIO_AF_4 -#define BST1_SCL_PIN GPIO_Pin_6 -#define BST1_SCL_PIN_SOURCE GPIO_PinSource6 -#define BST1_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOB -#define BST1_SDA_GPIO GPIOB -#define BST1_SDA_GPIO_AF GPIO_AF_4 -#define BST1_SDA_PIN GPIO_Pin_7 -#define BST1_SDA_PIN_SOURCE GPIO_PinSource7 -#define BST1_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOB +#define BST1_SCL_GPIO GPIOB +#define BST1_SCL_GPIO_AF GPIO_AF_4 +#define BST1_SCL_PIN GPIO_Pin_6 +#define BST1_SCL_PIN_SOURCE GPIO_PinSource6 +#define BST1_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOB +#define BST1_SDA_GPIO GPIOB +#define BST1_SDA_GPIO_AF GPIO_AF_4 +#define BST1_SDA_PIN GPIO_Pin_7 +#define BST1_SDA_PIN_SOURCE GPIO_PinSource7 +#define BST1_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOB #endif #if !defined(BST2_SCL_GPIO) -#define BST2_SCL_GPIO GPIOF -#define BST2_SCL_GPIO_AF GPIO_AF_4 -#define BST2_SCL_PIN GPIO_Pin_6 -#define BST2_SCL_PIN_SOURCE GPIO_PinSource6 -#define BST2_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOF -#define BST2_SDA_GPIO GPIOA -#define BST2_SDA_GPIO_AF GPIO_AF_4 -#define BST2_SDA_PIN GPIO_Pin_10 -#define BST2_SDA_PIN_SOURCE GPIO_PinSource10 -#define BST2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA +#define BST2_SCL_GPIO GPIOF +#define BST2_SCL_GPIO_AF GPIO_AF_4 +#define BST2_SCL_PIN GPIO_Pin_6 +#define BST2_SCL_PIN_SOURCE GPIO_PinSource6 +#define BST2_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOF +#define BST2_SDA_GPIO GPIOA +#define BST2_SDA_GPIO_AF GPIO_AF_4 +#define BST2_SDA_PIN GPIO_Pin_10 +#define BST2_SDA_PIN_SOURCE GPIO_PinSource10 +#define BST2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA #endif static volatile uint16_t bst1ErrorCount = 0; @@ -73,219 +75,219 @@ uint8_t bufferPointer = 0; bool cleanflight_data_ready = false; uint8_t interruptCounter = 0; -#define DEALY_SENDING_BYTE 40 +#define DELAY_SENDING_BYTE 40 void bstProcessInCommand(void); void I2C_EV_IRQHandler() { - 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(!cleanflight_data_ready) { - I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS); - return; - } - if(interruptCounter < DEALY_SENDING_BYTE) { - interruptCounter++; - I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS); - return; - } else { - interruptCounter = 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); - } + 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(!cleanflight_data_ready) { + I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS); + return; + } + if(interruptCounter < DELAY_SENDING_BYTE) { + interruptCounter++; + I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS); + return; + } else { + interruptCounter = 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(); + I2C_EV_IRQHandler(); } void I2C2_EV_IRQHandler() { - I2C_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; + 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*/) { - NVIC_InitTypeDef nvic; - 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_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_GeneralCallCmd(I2C1, ENABLE); + I2C_GeneralCallCmd(I2C1, ENABLE); - 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); + 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); - I2C_ITConfig(I2C1, I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ERRI, ENABLE); + I2C_ITConfig(I2C1, I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ERRI, ENABLE); - I2C_Cmd(I2C1, ENABLE); - } + I2C_Cmd(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); + 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); - 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_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_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 = BST2_SCL_PIN; - GPIO_Init(BST2_SCL_GPIO, &GPIO_InitStructure); + GPIO_InitStructure.GPIO_Pin = BST2_SCL_PIN; + GPIO_Init(BST2_SCL_GPIO, &GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = BST2_SDA_PIN; - GPIO_Init(BST2_SDA_GPIO, &GPIO_InitStructure); + GPIO_InitStructure.GPIO_Pin = BST2_SDA_PIN; + GPIO_Init(BST2_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_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(I2C2, &BST_InitStructure); + I2C_Init(I2C2, &BST_InitStructure); - I2C_GeneralCallCmd(I2C2, ENABLE); + 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); + 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_ITConfig(I2C2, I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ERRI, ENABLE); - I2C_Cmd(I2C2, ENABLE); - } + I2C_Cmd(I2C2, ENABLE); + } } void bstInit(BSTDevice index) @@ -312,83 +314,83 @@ uint16_t bstGetErrorCounter(void) bool bstWriteBusy(void) { - if(bstWriteDataLen) - return true; - else - return false; + if(bstWriteDataLen) + return true; + else + return false; } bool bstMasterWrite(uint8_t* data) { - if(bstWriteDataLen==0) { - CRC8 = 0; - dataBufferPointer = 0; - dataBuffer[0] = *data; - dataBuffer[1] = *(data+1); - bstWriteDataLen = dataBuffer[1] + 2; - for(uint8_t i=2; iIDR&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(); - } + 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 crc8Cal(uint8_t data_in) { - /* Polynom = x^8+x^7+x^6+x^4+x^2+1 = x^8+x^7+x^6+x^4+x^2+X^0 */ - uint8_t Polynom = BST_CRC_POLYNOM; - bool MSB_Flag; + /* Polynom = x^8+x^7+x^6+x^4+x^2+1 = x^8+x^7+x^6+x^4+x^2+X^0 */ + uint8_t Polynom = BST_CRC_POLYNOM; + bool MSB_Flag; - /* Step through each bit of the BYTE (8-bits) */ - for (uint8_t i = 0; i < 8; i++) { - /* Clear the Flag */ - MSB_Flag = false; + /* Step through each bit of the BYTE (8-bits) */ + for (uint8_t i = 0; i < 8; i++) { + /* Clear the Flag */ + MSB_Flag = false; - /* MSB_Set = 80; */ - if (CRC8 & 0x80) { - MSB_Flag = true; - } - - CRC8 <<= 1; + /* MSB_Set = 80; */ + if (CRC8 & 0x80) { + MSB_Flag = true; + } - /* MSB_Set = 80; */ - if (data_in & 0x80) { - CRC8++; - } - data_in <<= 1; - - if (MSB_Flag == true) { - CRC8 ^= Polynom; - } - } + CRC8 <<= 1; + + /* MSB_Set = 80; */ + if (data_in & 0x80) { + CRC8++; + } + data_in <<= 1; + + if (MSB_Flag == true) { + CRC8 ^= Polynom; + } + } } #endif diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 94a29e2d4..304db985e 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -193,14 +193,14 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; // DEPRECATED - Use BST_BUILD_INFO instead #define BST_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion -#define BST_DISARM 70 //in message to disarm -#define BST_ENABLE_ARM 71 //in message to enable arm +#define BST_DISARM 70 //in message to disarm +#define BST_ENABLE_ARM 71 //in message to enable arm -#define BST_DEADBAND 72 //out message -#define BST_SET_DEADBAND 73 //in message +#define BST_DEADBAND 72 //out message +#define BST_SET_DEADBAND 73 //in message -#define BST_FC_FILTERS 74 //out message -#define BST_SET_FC_FILTERS 75 //in message +#define BST_FC_FILTERS 74 //out message +#define BST_SET_FC_FILTERS 75 //in message // // Multwii original MSP commands @@ -291,12 +291,12 @@ static const char pidnames[] = "MAG;" "VEL;"; -#define BOARD_IDENTIFIER_LENGTH 4 +#define BOARD_IDENTIFIER_LENGTH 4 typedef struct box_e { - const uint8_t boxId; // see boxId_e - const char *boxName; // GUI-readable box name - const uint8_t permanentId; // + const uint8_t boxId; // see boxId_e + const char *boxName; // GUI-readable box name + const uint8_t permanentId; // } box_t; // FIXME remove ;'s @@ -338,26 +338,26 @@ extern uint8_t writeData[DATA_BUFFER_SIZE]; /*************************************************************************************************/ uint8_t writeBufferPointer = 1; static void bstWrite8(uint8_t data) { - writeData[writeBufferPointer++] = data; - writeData[0] = writeBufferPointer; + writeData[writeBufferPointer++] = data; + writeData[0] = writeBufferPointer; } static void bstWrite16(uint16_t data) { - bstWrite8((uint8_t)(data >> 0)); - bstWrite8((uint8_t)(data >> 8)); + bstWrite8((uint8_t)(data >> 0)); + bstWrite8((uint8_t)(data >> 8)); } static void bstWrite32(uint32_t data) { - bstWrite16((uint16_t)(data >> 0)); - bstWrite16((uint16_t)(data >> 16)); + bstWrite16((uint16_t)(data >> 0)); + bstWrite16((uint16_t)(data >> 16)); } uint8_t readBufferPointer = 4; static uint8_t bstCurrentAddress(void) { - return readData[0]; + return readData[0]; } static uint8_t bstRead8(void) @@ -381,25 +381,25 @@ static uint32_t bstRead32(void) static uint8_t bstReadDataSize(void) { - return readData[1]-5; + return readData[1]-5; } static uint8_t bstReadCRC(void) { - return readData[readData[1]+1]; + return readData[readData[1]+1]; } static void s_struct(uint8_t *cb, uint8_t siz) { while (siz--) - bstWrite8(*cb++); + bstWrite8(*cb++); } static void bstWriteNames(const char *s) { const char *c; for (c = s; *c; c++) - bstWrite8(*c); + bstWrite8(*c); } static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId) @@ -449,7 +449,7 @@ reset: count += len; } else { for (j = 0; j < len; j++) - bstWrite8(box->boxName[j]); + bstWrite8(box->boxName[j]); } } @@ -462,1053 +462,1053 @@ reset: static void bstWriteDataflashSummaryReply(void) { #ifdef USE_FLASHFS - const flashGeometry_t *geometry = flashfsGetGeometry(); - bstWrite8(flashfsIsReady() ? 1 : 0); - bstWrite32(geometry->sectors); - bstWrite32(geometry->totalSize); - bstWrite32(flashfsGetOffset()); // Effectively the current number of bytes stored on the volume + const flashGeometry_t *geometry = flashfsGetGeometry(); + bstWrite8(flashfsIsReady() ? 1 : 0); + bstWrite32(geometry->sectors); + bstWrite32(geometry->totalSize); + bstWrite32(flashfsGetOffset()); // Effectively the current number of bytes stored on the volume #else - bstWrite8(0); - bstWrite32(0); - bstWrite32(0); - bstWrite32(0); + bstWrite8(0); + bstWrite32(0); + bstWrite32(0); + bstWrite32(0); #endif } #ifdef USE_FLASHFS static void bstWriteDataflashReadReply(uint32_t address, uint8_t size) { - uint8_t buffer[128]; - int bytesRead; + uint8_t buffer[128]; + int bytesRead; - if (size > sizeof(buffer)) { - size = sizeof(buffer); - } + if (size > sizeof(buffer)) { + size = sizeof(buffer); + } - bstWrite32(address); + bstWrite32(address); - // bytesRead will be lower than that requested if we reach end of volume - bytesRead = flashfsReadAbs(address, buffer, size); + // bytesRead will be lower than that requested if we reach end of volume + bytesRead = flashfsReadAbs(address, buffer, size); - for (int i = 0; i < bytesRead; i++) { - bstWrite8(buffer[i]); - } + for (int i = 0; i < bytesRead; i++) { + bstWrite8(buffer[i]); + } } #endif #define IS_ENABLED(mask) (mask == 0 ? 0 : 1) /*************************************************************************************************/ -#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 -#define BST_WRITE_COMMANDS 0x25 -#define BST_PASSED 0x01 -#define BST_FAILED 0x00 +#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 +#define BST_WRITE_COMMANDS 0x25 +#define BST_PASSED 0x01 +#define BST_FAILED 0x00 static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) { - uint32_t i, tmp, junk; + uint32_t i, tmp, junk; #ifdef GPS - uint8_t wp_no; - int32_t lat = 0, lon = 0; + uint8_t wp_no; + int32_t lat = 0, lon = 0; #endif - switch(bstRequest) { - case BST_API_VERSION: - bstWrite8(BST_PROTOCOL_VERSION); + switch(bstRequest) { + case BST_API_VERSION: + bstWrite8(BST_PROTOCOL_VERSION); - bstWrite8(API_VERSION_MAJOR); - bstWrite8(API_VERSION_MINOR); - break; - case BST_FC_VARIANT: - for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) { - bstWrite8(flightControllerIdentifier[i]); - } - break; - case BST_FC_VERSION: - bstWrite8(FC_VERSION_MAJOR); - bstWrite8(FC_VERSION_MINOR); - bstWrite8(FC_VERSION_PATCH_LEVEL); - break; - case BST_BOARD_INFO: - for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) { - bstWrite8(boardIdentifier[i]); - } - break; - case BST_BUILD_INFO: - for (i = 0; i < BUILD_DATE_LENGTH; i++) { - bstWrite8(buildDate[i]); - } - for (i = 0; i < BUILD_TIME_LENGTH; i++) { - bstWrite8(buildTime[i]); - } + bstWrite8(API_VERSION_MAJOR); + bstWrite8(API_VERSION_MINOR); + break; + case BST_FC_VARIANT: + for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) { + bstWrite8(flightControllerIdentifier[i]); + } + break; + case BST_FC_VERSION: + bstWrite8(FC_VERSION_MAJOR); + bstWrite8(FC_VERSION_MINOR); + bstWrite8(FC_VERSION_PATCH_LEVEL); + break; + case BST_BOARD_INFO: + for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) { + bstWrite8(boardIdentifier[i]); + } + break; + case BST_BUILD_INFO: + for (i = 0; i < BUILD_DATE_LENGTH; i++) { + bstWrite8(buildDate[i]); + } + for (i = 0; i < BUILD_TIME_LENGTH; i++) { + bstWrite8(buildTime[i]); + } - for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) { - bstWrite8(shortGitRevision[i]); - } - break; - // DEPRECATED - Use MSP_API_VERSION - case BST_IDENT: - bstWrite8(MW_VERSION); - bstWrite8(masterConfig.mixerMode); - bstWrite8(BST_PROTOCOL_VERSION); - bstWrite32(CAP_DYNBALANCE); // "capability" - break; + for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) { + bstWrite8(shortGitRevision[i]); + } + break; + // DEPRECATED - Use MSP_API_VERSION + case BST_IDENT: + bstWrite8(MW_VERSION); + bstWrite8(masterConfig.mixerMode); + bstWrite8(BST_PROTOCOL_VERSION); + bstWrite32(CAP_DYNBALANCE); // "capability" + break; - case BST_STATUS: - bstWrite16(cycleTime); + case BST_STATUS: + bstWrite16(cycleTime); #ifdef USE_I2C - bstWrite16(i2cGetErrorCounter()); + bstWrite16(i2cGetErrorCounter()); #else - bstWrite16(0); + bstWrite16(0); #endif - bstWrite16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); - // BST the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES - // Requires new Multiwii protocol version to fix - // It would be preferable to setting the enabled bits based on BOXINDEX. - junk = 0; - tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE | - IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON | - IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO | - IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG | - IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG | - IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE)) << BOXGPSHOME | - IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD | - IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE | - IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | - IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | - IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE; - for (i = 0; i < activeBoxIdCount; i++) { - int flag = (tmp & (1 << activeBoxIds[i])); - if (flag) - junk |= 1 << i; - } - bstWrite32(junk); - bstWrite8(masterConfig.current_profile_index); - break; - case BST_RAW_IMU: - { - // Hack scale due to choice of units for sensor data in multiwii - uint8_t scale = (acc.acc_1G > 1024) ? 8 : 1; + bstWrite16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); + // BST the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES + // Requires new Multiwii protocol version to fix + // It would be preferable to setting the enabled bits based on BOXINDEX. + junk = 0; + tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE | + IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON | + IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO | + IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG | + IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG | + IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE)) << BOXGPSHOME | + IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD | + IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE | + IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | + IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | + IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE; + for (i = 0; i < activeBoxIdCount; i++) { + int flag = (tmp & (1 << activeBoxIds[i])); + if (flag) + junk |= 1 << i; + } + bstWrite32(junk); + bstWrite8(masterConfig.current_profile_index); + break; + case BST_RAW_IMU: + { + // Hack scale due to choice of units for sensor data in multiwii + uint8_t scale = (acc.acc_1G > 1024) ? 8 : 1; - for (i = 0; i < 3; i++) - bstWrite16(accSmooth[i] / scale); - for (i = 0; i < 3; i++) - bstWrite16(gyroADC[i]); - for (i = 0; i < 3; i++) - bstWrite16(magADC[i]); - } - break; + for (i = 0; i < 3; i++) + bstWrite16(accSmooth[i] / scale); + for (i = 0; i < 3; i++) + bstWrite16(gyroADC[i]); + for (i = 0; i < 3; i++) + bstWrite16(magADC[i]); + } + break; #ifdef USE_SERVOS - case BST_SERVO: - s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2); - break; - case BST_SERVO_CONFIGURATIONS: - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - bstWrite16(masterConfig.servoConf[i].min); - bstWrite16(masterConfig.servoConf[i].max); - bstWrite16(masterConfig.servoConf[i].middle); - bstWrite8(masterConfig.servoConf[i].rate); - bstWrite8(masterConfig.servoConf[i].angleAtMin); - bstWrite8(masterConfig.servoConf[i].angleAtMax); - bstWrite8(masterConfig.servoConf[i].forwardFromChannel); - bstWrite32(masterConfig.servoConf[i].reversedSources); - } - break; - case BST_SERVO_MIX_RULES: - for (i = 0; i < MAX_SERVO_RULES; i++) { - bstWrite8(masterConfig.customServoMixer[i].targetChannel); - bstWrite8(masterConfig.customServoMixer[i].inputSource); - bstWrite8(masterConfig.customServoMixer[i].rate); - bstWrite8(masterConfig.customServoMixer[i].speed); - bstWrite8(masterConfig.customServoMixer[i].min); - bstWrite8(masterConfig.customServoMixer[i].max); - bstWrite8(masterConfig.customServoMixer[i].box); - } - break; + case BST_SERVO: + s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2); + break; + case BST_SERVO_CONFIGURATIONS: + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + bstWrite16(masterConfig.servoConf[i].min); + bstWrite16(masterConfig.servoConf[i].max); + bstWrite16(masterConfig.servoConf[i].middle); + bstWrite8(masterConfig.servoConf[i].rate); + bstWrite8(masterConfig.servoConf[i].angleAtMin); + bstWrite8(masterConfig.servoConf[i].angleAtMax); + bstWrite8(masterConfig.servoConf[i].forwardFromChannel); + bstWrite32(masterConfig.servoConf[i].reversedSources); + } + break; + case BST_SERVO_MIX_RULES: + for (i = 0; i < MAX_SERVO_RULES; i++) { + bstWrite8(masterConfig.customServoMixer[i].targetChannel); + bstWrite8(masterConfig.customServoMixer[i].inputSource); + bstWrite8(masterConfig.customServoMixer[i].rate); + bstWrite8(masterConfig.customServoMixer[i].speed); + bstWrite8(masterConfig.customServoMixer[i].min); + bstWrite8(masterConfig.customServoMixer[i].max); + bstWrite8(masterConfig.customServoMixer[i].box); + } + break; #endif - case BST_MOTOR: - s_struct((uint8_t *)motor, 16); - break; - case BST_RC: - for (i = 0; i < rxRuntimeConfig.channelCount; i++) - bstWrite16(rcData[i]); - break; - case BST_ATTITUDE: - for (i = 0; i < 2; i++) - bstWrite16(attitude.raw[i]); - //bstWrite16(heading); - break; - case BST_ALTITUDE: + case BST_MOTOR: + s_struct((uint8_t *)motor, 16); + break; + case BST_RC: + for (i = 0; i < rxRuntimeConfig.channelCount; i++) + bstWrite16(rcData[i]); + break; + case BST_ATTITUDE: + for (i = 0; i < 2; i++) + bstWrite16(attitude.raw[i]); + //bstWrite16(heading); + break; + case BST_ALTITUDE: #if defined(BARO) || defined(SONAR) - bstWrite32(altitudeHoldGetEstimatedAltitude()); + bstWrite32(altitudeHoldGetEstimatedAltitude()); #else - bstWrite32(0); + bstWrite32(0); #endif - bstWrite16(vario); - break; - case BST_SONAR_ALTITUDE: + bstWrite16(vario); + break; + case BST_SONAR_ALTITUDE: #if defined(SONAR) - bstWrite32(sonarGetLatestAltitude()); + bstWrite32(sonarGetLatestAltitude()); #else - bstWrite32(0); + bstWrite32(0); #endif - break; - case BST_ANALOG: - bstWrite8((uint8_t)constrain(vbat, 0, 255)); - bstWrite16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery - bstWrite16(rssi); - if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) { - bstWrite16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero - } else - bstWrite16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A - break; - case BST_ARMING_CONFIG: - bstWrite8(masterConfig.auto_disarm_delay); - bstWrite8(masterConfig.disarm_kill_switch); - break; - case BST_LOOP_TIME: - //bstWrite16(masterConfig.looptime); - bstWrite16(cycleTime); - break; - case BST_RC_TUNING: - bstWrite8(currentControlRateProfile->rcRate8); - bstWrite8(currentControlRateProfile->rcExpo8); - for (i = 0 ; i < 3; i++) { - bstWrite8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t - } - bstWrite8(currentControlRateProfile->dynThrPID); - bstWrite8(currentControlRateProfile->thrMid8); - bstWrite8(currentControlRateProfile->thrExpo8); - bstWrite16(currentControlRateProfile->tpa_breakpoint); - bstWrite8(currentControlRateProfile->rcYawExpo8); - break; - case BST_PID: - 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: - bstWriteNames(pidnames); - break; - case BST_PID_CONTROLLER: - bstWrite8(currentProfile->pidProfile.pidController); - break; - case BST_MODE_RANGES: - for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { - modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; - const box_t *box = &boxes[mac->modeId]; - bstWrite8(box->permanentId); - bstWrite8(mac->auxChannelIndex); - bstWrite8(mac->range.startStep); - bstWrite8(mac->range.endStep); - } - break; - case BST_ADJUSTMENT_RANGES: - for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { - adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i]; - bstWrite8(adjRange->adjustmentIndex); - bstWrite8(adjRange->auxChannelIndex); - bstWrite8(adjRange->range.startStep); - bstWrite8(adjRange->range.endStep); - bstWrite8(adjRange->adjustmentFunction); - bstWrite8(adjRange->auxSwitchChannelIndex); - } - break; - case BST_BOXNAMES: - bstWriteBoxNamesReply(); - break; - case BST_BOXIDS: - for (i = 0; i < activeBoxIdCount; i++) { - const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]); - if (!box) { - continue; - } - bstWrite8(box->permanentId); - } - break; - case BST_MISC: - bstWrite16(masterConfig.rxConfig.midrc); + break; + case BST_ANALOG: + bstWrite8((uint8_t)constrain(vbat, 0, 255)); + bstWrite16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery + bstWrite16(rssi); + if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) { + bstWrite16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero + } else + bstWrite16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A + break; + case BST_ARMING_CONFIG: + bstWrite8(masterConfig.auto_disarm_delay); + bstWrite8(masterConfig.disarm_kill_switch); + break; + case BST_LOOP_TIME: + //bstWrite16(masterConfig.looptime); + bstWrite16(cycleTime); + break; + case BST_RC_TUNING: + bstWrite8(currentControlRateProfile->rcRate8); + bstWrite8(currentControlRateProfile->rcExpo8); + for (i = 0 ; i < 3; i++) { + bstWrite8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t + } + bstWrite8(currentControlRateProfile->dynThrPID); + bstWrite8(currentControlRateProfile->thrMid8); + bstWrite8(currentControlRateProfile->thrExpo8); + bstWrite16(currentControlRateProfile->tpa_breakpoint); + bstWrite8(currentControlRateProfile->rcYawExpo8); + break; + case BST_PID: + 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: + bstWriteNames(pidnames); + break; + case BST_PID_CONTROLLER: + bstWrite8(currentProfile->pidProfile.pidController); + break; + case BST_MODE_RANGES: + for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; + const box_t *box = &boxes[mac->modeId]; + bstWrite8(box->permanentId); + bstWrite8(mac->auxChannelIndex); + bstWrite8(mac->range.startStep); + bstWrite8(mac->range.endStep); + } + break; + case BST_ADJUSTMENT_RANGES: + for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { + adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i]; + bstWrite8(adjRange->adjustmentIndex); + bstWrite8(adjRange->auxChannelIndex); + bstWrite8(adjRange->range.startStep); + bstWrite8(adjRange->range.endStep); + bstWrite8(adjRange->adjustmentFunction); + bstWrite8(adjRange->auxSwitchChannelIndex); + } + break; + case BST_BOXNAMES: + bstWriteBoxNamesReply(); + break; + case BST_BOXIDS: + for (i = 0; i < activeBoxIdCount; i++) { + const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]); + if (!box) { + continue; + } + bstWrite8(box->permanentId); + } + break; + case BST_MISC: + bstWrite16(masterConfig.rxConfig.midrc); - bstWrite16(masterConfig.escAndServoConfig.minthrottle); - bstWrite16(masterConfig.escAndServoConfig.maxthrottle); - bstWrite16(masterConfig.escAndServoConfig.mincommand); + bstWrite16(masterConfig.escAndServoConfig.minthrottle); + bstWrite16(masterConfig.escAndServoConfig.maxthrottle); + bstWrite16(masterConfig.escAndServoConfig.mincommand); - bstWrite16(masterConfig.failsafeConfig.failsafe_throttle); + bstWrite16(masterConfig.failsafeConfig.failsafe_throttle); #ifdef GPS - bstWrite8(masterConfig.gpsConfig.provider); // gps_type - bstWrite8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t - bstWrite8(masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas + bstWrite8(masterConfig.gpsConfig.provider); // gps_type + bstWrite8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t + bstWrite8(masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas #else - bstWrite8(0); // gps_type - bstWrite8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t - bstWrite8(0); // gps_ubx_sbas + bstWrite8(0); // gps_type + bstWrite8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t + bstWrite8(0); // gps_ubx_sbas #endif - bstWrite8(masterConfig.batteryConfig.multiwiiCurrentMeterOutput); - bstWrite8(masterConfig.rxConfig.rssi_channel); - bstWrite8(0); + bstWrite8(masterConfig.batteryConfig.multiwiiCurrentMeterOutput); + bstWrite8(masterConfig.rxConfig.rssi_channel); + bstWrite8(0); - bstWrite16(masterConfig.mag_declination / 10); + bstWrite16(masterConfig.mag_declination / 10); - bstWrite8(masterConfig.batteryConfig.vbatscale); - bstWrite8(masterConfig.batteryConfig.vbatmincellvoltage); - bstWrite8(masterConfig.batteryConfig.vbatmaxcellvoltage); - bstWrite8(masterConfig.batteryConfig.vbatwarningcellvoltage); - break; - case BST_MOTOR_PINS: - // FIXME This is hardcoded and should not be. - for (i = 0; i < 8; i++) - bstWrite8(i + 1); - break; + bstWrite8(masterConfig.batteryConfig.vbatscale); + bstWrite8(masterConfig.batteryConfig.vbatmincellvoltage); + bstWrite8(masterConfig.batteryConfig.vbatmaxcellvoltage); + bstWrite8(masterConfig.batteryConfig.vbatwarningcellvoltage); + break; + case BST_MOTOR_PINS: + // FIXME This is hardcoded and should not be. + for (i = 0; i < 8; i++) + bstWrite8(i + 1); + break; #ifdef GPS - case BST_RAW_GPS: - bstWrite8(STATE(GPS_FIX)); - bstWrite8(GPS_numSat); - bstWrite32(GPS_coord[LAT]); - bstWrite32(GPS_coord[LON]); - bstWrite16(GPS_altitude); - bstWrite16(GPS_speed); - bstWrite16(GPS_ground_course); - break; - case BST_COMP_GPS: - bstWrite16(GPS_distanceToHome); - bstWrite16(GPS_directionToHome); - bstWrite8(GPS_update & 1); - break; - case BST_WP: - wp_no = bstRead8(); // get the wp number - if (wp_no == 0) { - lat = GPS_home[LAT]; - lon = GPS_home[LON]; - } else if (wp_no == 16) { - lat = GPS_hold[LAT]; - lon = GPS_hold[LON]; - } - bstWrite8(wp_no); - bstWrite32(lat); - bstWrite32(lon); - bstWrite32(AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps - bstWrite16(0); // heading will come here (deg) - bstWrite16(0); // time to stay (ms) will come here - bstWrite8(0); // nav flag will come here - break; - case BST_GPSSVINFO: - bstWrite8(GPS_numCh); - for (i = 0; i < GPS_numCh; i++){ - bstWrite8(GPS_svinfo_chn[i]); - bstWrite8(GPS_svinfo_svid[i]); - bstWrite8(GPS_svinfo_quality[i]); - bstWrite8(GPS_svinfo_cno[i]); - } - break; + case BST_RAW_GPS: + bstWrite8(STATE(GPS_FIX)); + bstWrite8(GPS_numSat); + bstWrite32(GPS_coord[LAT]); + bstWrite32(GPS_coord[LON]); + bstWrite16(GPS_altitude); + bstWrite16(GPS_speed); + bstWrite16(GPS_ground_course); + break; + case BST_COMP_GPS: + bstWrite16(GPS_distanceToHome); + bstWrite16(GPS_directionToHome); + bstWrite8(GPS_update & 1); + break; + case BST_WP: + wp_no = bstRead8(); // get the wp number + if (wp_no == 0) { + lat = GPS_home[LAT]; + lon = GPS_home[LON]; + } else if (wp_no == 16) { + lat = GPS_hold[LAT]; + lon = GPS_hold[LON]; + } + bstWrite8(wp_no); + bstWrite32(lat); + bstWrite32(lon); + bstWrite32(AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps + bstWrite16(0); // heading will come here (deg) + bstWrite16(0); // time to stay (ms) will come here + bstWrite8(0); // nav flag will come here + break; + case BST_GPSSVINFO: + bstWrite8(GPS_numCh); + for (i = 0; i < GPS_numCh; i++){ + bstWrite8(GPS_svinfo_chn[i]); + bstWrite8(GPS_svinfo_svid[i]); + bstWrite8(GPS_svinfo_quality[i]); + bstWrite8(GPS_svinfo_cno[i]); + } + break; #endif - case BST_DEBUG: - // output some useful QA statistics - // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] + case BST_DEBUG: + // output some useful QA statistics + // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] - for (i = 0; i < DEBUG16_VALUE_COUNT; i++) - bstWrite16(debug[i]); // 4 variables are here for general monitoring purpose - break; + for (i = 0; i < DEBUG16_VALUE_COUNT; i++) + bstWrite16(debug[i]); // 4 variables are here for general monitoring purpose + break; - // Additional commands that are not compatible with MultiWii - case BST_ACC_TRIM: - bstWrite16(masterConfig.accelerometerTrims.values.pitch); - bstWrite16(masterConfig.accelerometerTrims.values.roll); - break; + // Additional commands that are not compatible with MultiWii + case BST_ACC_TRIM: + bstWrite16(masterConfig.accelerometerTrims.values.pitch); + bstWrite16(masterConfig.accelerometerTrims.values.roll); + break; - case BST_UID: - bstWrite32(U_ID_0); - bstWrite32(U_ID_1); - bstWrite32(U_ID_2); - break; + case BST_UID: + bstWrite32(U_ID_0); + bstWrite32(U_ID_1); + bstWrite32(U_ID_2); + break; - case BST_FEATURE: - bstWrite32(featureMask()); - break; + case BST_FEATURE: + bstWrite32(featureMask()); + break; - case BST_BOARD_ALIGNMENT: - bstWrite16(masterConfig.boardAlignment.rollDegrees); - bstWrite16(masterConfig.boardAlignment.pitchDegrees); - bstWrite16(masterConfig.boardAlignment.yawDegrees); - break; + case BST_BOARD_ALIGNMENT: + bstWrite16(masterConfig.boardAlignment.rollDegrees); + bstWrite16(masterConfig.boardAlignment.pitchDegrees); + bstWrite16(masterConfig.boardAlignment.yawDegrees); + break; - case BST_VOLTAGE_METER_CONFIG: - bstWrite8(masterConfig.batteryConfig.vbatscale); - bstWrite8(masterConfig.batteryConfig.vbatmincellvoltage); - bstWrite8(masterConfig.batteryConfig.vbatmaxcellvoltage); - bstWrite8(masterConfig.batteryConfig.vbatwarningcellvoltage); - break; + case BST_VOLTAGE_METER_CONFIG: + bstWrite8(masterConfig.batteryConfig.vbatscale); + bstWrite8(masterConfig.batteryConfig.vbatmincellvoltage); + bstWrite8(masterConfig.batteryConfig.vbatmaxcellvoltage); + bstWrite8(masterConfig.batteryConfig.vbatwarningcellvoltage); + break; - case BST_CURRENT_METER_CONFIG: - bstWrite16(masterConfig.batteryConfig.currentMeterScale); - bstWrite16(masterConfig.batteryConfig.currentMeterOffset); - bstWrite8(masterConfig.batteryConfig.currentMeterType); - bstWrite16(masterConfig.batteryConfig.batteryCapacity); - break; + case BST_CURRENT_METER_CONFIG: + bstWrite16(masterConfig.batteryConfig.currentMeterScale); + bstWrite16(masterConfig.batteryConfig.currentMeterOffset); + bstWrite8(masterConfig.batteryConfig.currentMeterType); + bstWrite16(masterConfig.batteryConfig.batteryCapacity); + break; - case BST_MIXER: - bstWrite8(masterConfig.mixerMode); - break; + case BST_MIXER: + bstWrite8(masterConfig.mixerMode); + break; - case BST_RX_CONFIG: - bstWrite8(masterConfig.rxConfig.serialrx_provider); - bstWrite16(masterConfig.rxConfig.maxcheck); - bstWrite16(masterConfig.rxConfig.midrc); - bstWrite16(masterConfig.rxConfig.mincheck); - bstWrite8(masterConfig.rxConfig.spektrum_sat_bind); - bstWrite16(masterConfig.rxConfig.rx_min_usec); - bstWrite16(masterConfig.rxConfig.rx_max_usec); - break; + case BST_RX_CONFIG: + bstWrite8(masterConfig.rxConfig.serialrx_provider); + bstWrite16(masterConfig.rxConfig.maxcheck); + bstWrite16(masterConfig.rxConfig.midrc); + bstWrite16(masterConfig.rxConfig.mincheck); + bstWrite8(masterConfig.rxConfig.spektrum_sat_bind); + bstWrite16(masterConfig.rxConfig.rx_min_usec); + bstWrite16(masterConfig.rxConfig.rx_max_usec); + break; - case BST_FAILSAFE_CONFIG: - bstWrite8(masterConfig.failsafeConfig.failsafe_delay); - bstWrite8(masterConfig.failsafeConfig.failsafe_off_delay); - bstWrite16(masterConfig.failsafeConfig.failsafe_throttle); - break; + case BST_FAILSAFE_CONFIG: + bstWrite8(masterConfig.failsafeConfig.failsafe_delay); + bstWrite8(masterConfig.failsafeConfig.failsafe_off_delay); + bstWrite16(masterConfig.failsafeConfig.failsafe_throttle); + break; - case BST_RXFAIL_CONFIG: - for (i = NON_AUX_CHANNEL_COUNT; i < rxRuntimeConfig.channelCount; i++) { - bstWrite8(masterConfig.rxConfig.failsafe_channel_configurations[i].mode); - bstWrite16(RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_channel_configurations[i].step)); - } - break; + case BST_RXFAIL_CONFIG: + for (i = NON_AUX_CHANNEL_COUNT; i < rxRuntimeConfig.channelCount; i++) { + bstWrite8(masterConfig.rxConfig.failsafe_channel_configurations[i].mode); + bstWrite16(RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_channel_configurations[i].step)); + } + break; - case BST_RSSI_CONFIG: - bstWrite8(masterConfig.rxConfig.rssi_channel); - break; + case BST_RSSI_CONFIG: + bstWrite8(masterConfig.rxConfig.rssi_channel); + break; - case BST_RX_MAP: - for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) - bstWrite8(masterConfig.rxConfig.rcmap[i]); - break; + case BST_RX_MAP: + for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) + bstWrite8(masterConfig.rxConfig.rcmap[i]); + break; - case BST_BF_CONFIG: - bstWrite8(masterConfig.mixerMode); + case BST_BF_CONFIG: + bstWrite8(masterConfig.mixerMode); - bstWrite32(featureMask()); + bstWrite32(featureMask()); - bstWrite8(masterConfig.rxConfig.serialrx_provider); + bstWrite8(masterConfig.rxConfig.serialrx_provider); - bstWrite16(masterConfig.boardAlignment.rollDegrees); - bstWrite16(masterConfig.boardAlignment.pitchDegrees); - bstWrite16(masterConfig.boardAlignment.yawDegrees); + bstWrite16(masterConfig.boardAlignment.rollDegrees); + bstWrite16(masterConfig.boardAlignment.pitchDegrees); + bstWrite16(masterConfig.boardAlignment.yawDegrees); - bstWrite16(masterConfig.batteryConfig.currentMeterScale); - bstWrite16(masterConfig.batteryConfig.currentMeterOffset); - break; + bstWrite16(masterConfig.batteryConfig.currentMeterScale); + bstWrite16(masterConfig.batteryConfig.currentMeterOffset); + break; - case BST_CF_SERIAL_CONFIG: - for (i = 0; i < SERIAL_PORT_COUNT; i++) { - if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) { - continue; - }; - bstWrite8(masterConfig.serialConfig.portConfigs[i].identifier); - bstWrite16(masterConfig.serialConfig.portConfigs[i].functionMask); - bstWrite8(masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex); - bstWrite8(masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex); - bstWrite8(masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex); - bstWrite8(masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex); - } - break; + case BST_CF_SERIAL_CONFIG: + for (i = 0; i < SERIAL_PORT_COUNT; i++) { + if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) { + continue; + }; + bstWrite8(masterConfig.serialConfig.portConfigs[i].identifier); + bstWrite16(masterConfig.serialConfig.portConfigs[i].functionMask); + bstWrite8(masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex); + bstWrite8(masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex); + bstWrite8(masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex); + bstWrite8(masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex); + } + break; #ifdef LED_STRIP - case BST_LED_COLORS: - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &masterConfig.colors[i]; - bstWrite16(color->h); - bstWrite8(color->s); - bstWrite8(color->v); - } - break; + case BST_LED_COLORS: + for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + hsvColor_t *color = &masterConfig.colors[i]; + bstWrite16(color->h); + bstWrite8(color->s); + bstWrite8(color->v); + } + break; - case BST_LED_STRIP_CONFIG: - for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - bstWrite16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET); - bstWrite16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET); - bstWrite8(GET_LED_X(ledConfig)); - bstWrite8(GET_LED_Y(ledConfig)); - bstWrite8(ledConfig->color); - } - break; + case BST_LED_STRIP_CONFIG: + for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { + ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + bstWrite16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET); + bstWrite16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET); + bstWrite8(GET_LED_X(ledConfig)); + bstWrite8(GET_LED_Y(ledConfig)); + bstWrite8(ledConfig->color); + } + break; #endif - case BST_DATAFLASH_SUMMARY: - bstWriteDataflashSummaryReply(); - break; + case BST_DATAFLASH_SUMMARY: + bstWriteDataflashSummaryReply(); + break; #ifdef USE_FLASHFS - case BST_DATAFLASH_READ: - { - uint32_t readAddress = bstRead32(); + case BST_DATAFLASH_READ: + { + uint32_t readAddress = bstRead32(); - bstWriteDataflashReadReply(readAddress, 128); - } - break; + bstWriteDataflashReadReply(readAddress, 128); + } + break; #endif - case BST_BF_BUILD_INFO: - for (i = 0; i < 11; i++) - bstWrite8(buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc - bstWrite32(0); // future exp - bstWrite32(0); // future exp - break; - case BST_DEADBAND: - bstWrite8(masterConfig.rcControlsConfig.alt_hold_deadband); - bstWrite8(masterConfig.rcControlsConfig.alt_hold_fast_change); - bstWrite8(masterConfig.rcControlsConfig.deadband); - bstWrite8(masterConfig.rcControlsConfig.yaw_deadband); - break; - case BST_FC_FILTERS: - bstWrite16(constrain(masterConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values - break; - default: - // we do not know how to handle the (valid) message, indicate error BST - return false; - } - //bstSlaveWrite(writeData); - return true; + case BST_BF_BUILD_INFO: + for (i = 0; i < 11; i++) + bstWrite8(buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc + bstWrite32(0); // future exp + bstWrite32(0); // future exp + break; + case BST_DEADBAND: + bstWrite8(masterConfig.rcControlsConfig.alt_hold_deadband); + bstWrite8(masterConfig.rcControlsConfig.alt_hold_fast_change); + bstWrite8(masterConfig.rcControlsConfig.deadband); + bstWrite8(masterConfig.rcControlsConfig.yaw_deadband); + break; + case BST_FC_FILTERS: + bstWrite16(constrain(masterConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values + break; + default: + // we do not know how to handle the (valid) message, indicate error BST + return false; + } + //bstSlaveWrite(writeData); + return true; } static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) { - uint32_t i; - uint16_t tmp; - uint8_t rate; + uint32_t i; + uint16_t tmp; + uint8_t rate; #ifdef GPS - uint8_t wp_no; - int32_t lat = 0, lon = 0, alt = 0; + uint8_t wp_no; + int32_t lat = 0, lon = 0, alt = 0; #endif - bool ret = BST_PASSED; - switch(bstWriteCommand) { - case BST_SELECT_SETTING: - if (!ARMING_FLAG(ARMED)) { - masterConfig.current_profile_index = bstRead8(); - if (masterConfig.current_profile_index > 2) { - masterConfig.current_profile_index = 0; - } - writeEEPROM(); - readEEPROM(); - } - break; - case BST_SET_HEAD: - magHold = bstRead16(); - break; - case BST_SET_RAW_RC: - { - uint8_t channelCount = bstReadDataSize() / sizeof(uint16_t); - if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) { - ret = BST_FAILED; - } else { - uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + bool ret = BST_PASSED; + switch(bstWriteCommand) { + case BST_SELECT_SETTING: + if (!ARMING_FLAG(ARMED)) { + masterConfig.current_profile_index = bstRead8(); + if (masterConfig.current_profile_index > 2) { + masterConfig.current_profile_index = 0; + } + writeEEPROM(); + readEEPROM(); + } + break; + case BST_SET_HEAD: + magHold = bstRead16(); + break; + case BST_SET_RAW_RC: + { + uint8_t channelCount = bstReadDataSize() / sizeof(uint16_t); + if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) { + ret = BST_FAILED; + } else { + uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; - for (i = 0; i < channelCount; i++) { - frame[i] = bstRead16(); - } + for (i = 0; i < channelCount; i++) { + frame[i] = bstRead16(); + } - rxMspFrameReceive(frame, channelCount); - } - } - case BST_SET_ACC_TRIM: - masterConfig.accelerometerTrims.values.pitch = bstRead16(); - masterConfig.accelerometerTrims.values.roll = bstRead16(); - break; - case BST_SET_ARMING_CONFIG: - masterConfig.auto_disarm_delay = bstRead8(); - masterConfig.disarm_kill_switch = bstRead8(); - break; - case BST_SET_LOOP_TIME: - //masterConfig.looptime = bstRead16(); - cycleTime = bstRead16(); - break; - case BST_SET_PID_CONTROLLER: - currentProfile->pidProfile.pidController = bstRead8(); - pidSetController(currentProfile->pidProfile.pidController); - break; - case BST_SET_PID: - 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(); - if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { - modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; - i = bstRead8(); - const box_t *box = findBoxByPermenantId(i); - if (box) { - mac->modeId = box->boxId; - mac->auxChannelIndex = bstRead8(); - mac->range.startStep = bstRead8(); - mac->range.endStep = bstRead8(); + rxMspFrameReceive(frame, channelCount); + } + } + case BST_SET_ACC_TRIM: + masterConfig.accelerometerTrims.values.pitch = bstRead16(); + masterConfig.accelerometerTrims.values.roll = bstRead16(); + break; + case BST_SET_ARMING_CONFIG: + masterConfig.auto_disarm_delay = bstRead8(); + masterConfig.disarm_kill_switch = bstRead8(); + break; + case BST_SET_LOOP_TIME: + //masterConfig.looptime = bstRead16(); + cycleTime = bstRead16(); + break; + case BST_SET_PID_CONTROLLER: + currentProfile->pidProfile.pidController = bstRead8(); + pidSetController(currentProfile->pidProfile.pidController); + break; + case BST_SET_PID: + 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(); + if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { + modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; + i = bstRead8(); + const box_t *box = findBoxByPermenantId(i); + if (box) { + mac->modeId = box->boxId; + mac->auxChannelIndex = bstRead8(); + mac->range.startStep = bstRead8(); + mac->range.endStep = bstRead8(); - useRcControlsConfig(masterConfig.modeActivationConditions, &masterConfig.escAndServoConfig, ¤tProfile->pidProfile); - } else { - ret = BST_FAILED; - } - } else { - ret = BST_FAILED; - } - break; - case BST_SET_ADJUSTMENT_RANGE: - i = bstRead8(); - if (i < MAX_ADJUSTMENT_RANGE_COUNT) { - adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i]; - i = bstRead8(); - if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { - adjRange->adjustmentIndex = i; - adjRange->auxChannelIndex = bstRead8(); - adjRange->range.startStep = bstRead8(); - adjRange->range.endStep = bstRead8(); - adjRange->adjustmentFunction = bstRead8(); - adjRange->auxSwitchChannelIndex = bstRead8(); - } else { - ret = BST_FAILED; - } - } else { - ret = BST_FAILED; - } - break; - case BST_SET_RC_TUNING: - if (bstReadDataSize() >= 10) { - currentControlRateProfile->rcRate8 = bstRead8(); - currentControlRateProfile->rcExpo8 = bstRead8(); - for (i = 0; i < 3; i++) { - rate = bstRead8(); - currentControlRateProfile->rates[i] = MIN(rate, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); - } - rate = bstRead8(); - currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); - currentControlRateProfile->thrMid8 = bstRead8(); - currentControlRateProfile->thrExpo8 = bstRead8(); - currentControlRateProfile->tpa_breakpoint = bstRead16(); - if (bstReadDataSize() >= 11) { - currentControlRateProfile->rcYawExpo8 = bstRead8(); - } - } else { - ret = BST_FAILED; - } - break; - case BST_SET_MISC: - tmp = bstRead16(); - if (tmp < 1600 && tmp > 1400) - masterConfig.rxConfig.midrc = tmp; + useRcControlsConfig(masterConfig.modeActivationConditions, &masterConfig.escAndServoConfig, ¤tProfile->pidProfile); + } else { + ret = BST_FAILED; + } + } else { + ret = BST_FAILED; + } + break; + case BST_SET_ADJUSTMENT_RANGE: + i = bstRead8(); + if (i < MAX_ADJUSTMENT_RANGE_COUNT) { + adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i]; + i = bstRead8(); + if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { + adjRange->adjustmentIndex = i; + adjRange->auxChannelIndex = bstRead8(); + adjRange->range.startStep = bstRead8(); + adjRange->range.endStep = bstRead8(); + adjRange->adjustmentFunction = bstRead8(); + adjRange->auxSwitchChannelIndex = bstRead8(); + } else { + ret = BST_FAILED; + } + } else { + ret = BST_FAILED; + } + break; + case BST_SET_RC_TUNING: + if (bstReadDataSize() >= 10) { + currentControlRateProfile->rcRate8 = bstRead8(); + currentControlRateProfile->rcExpo8 = bstRead8(); + for (i = 0; i < 3; i++) { + rate = bstRead8(); + currentControlRateProfile->rates[i] = MIN(rate, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + } + rate = bstRead8(); + currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); + currentControlRateProfile->thrMid8 = bstRead8(); + currentControlRateProfile->thrExpo8 = bstRead8(); + currentControlRateProfile->tpa_breakpoint = bstRead16(); + if (bstReadDataSize() >= 11) { + currentControlRateProfile->rcYawExpo8 = bstRead8(); + } + } else { + ret = BST_FAILED; + } + break; + case BST_SET_MISC: + tmp = bstRead16(); + if (tmp < 1600 && tmp > 1400) + masterConfig.rxConfig.midrc = tmp; - masterConfig.escAndServoConfig.minthrottle = bstRead16(); - masterConfig.escAndServoConfig.maxthrottle = bstRead16(); - masterConfig.escAndServoConfig.mincommand = bstRead16(); + masterConfig.escAndServoConfig.minthrottle = bstRead16(); + masterConfig.escAndServoConfig.maxthrottle = bstRead16(); + masterConfig.escAndServoConfig.mincommand = bstRead16(); - masterConfig.failsafeConfig.failsafe_throttle = bstRead16(); + masterConfig.failsafeConfig.failsafe_throttle = bstRead16(); - #ifdef GPS - masterConfig.gpsConfig.provider = bstRead8(); // gps_type - bstRead8(); // gps_baudrate - masterConfig.gpsConfig.sbasMode = bstRead8(); // gps_ubx_sbas - #else - bstRead8(); // gps_type - bstRead8(); // gps_baudrate - bstRead8(); // gps_ubx_sbas - #endif - masterConfig.batteryConfig.multiwiiCurrentMeterOutput = bstRead8(); - masterConfig.rxConfig.rssi_channel = bstRead8(); - bstRead8(); + #ifdef GPS + masterConfig.gpsConfig.provider = bstRead8(); // gps_type + bstRead8(); // gps_baudrate + masterConfig.gpsConfig.sbasMode = bstRead8(); // gps_ubx_sbas + #else + bstRead8(); // gps_type + bstRead8(); // gps_baudrate + bstRead8(); // gps_ubx_sbas + #endif + masterConfig.batteryConfig.multiwiiCurrentMeterOutput = bstRead8(); + masterConfig.rxConfig.rssi_channel = bstRead8(); + bstRead8(); - masterConfig.mag_declination = bstRead16() * 10; + masterConfig.mag_declination = bstRead16() * 10; - masterConfig.batteryConfig.vbatscale = bstRead8(); // actual vbatscale as intended - masterConfig.batteryConfig.vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI - masterConfig.batteryConfig.vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI - masterConfig.batteryConfig.vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert - break; - case BST_SET_MOTOR: - for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 - motor_disarmed[i] = bstRead16(); - break; - case BST_SET_SERVO_CONFIGURATION: + masterConfig.batteryConfig.vbatscale = bstRead8(); // actual vbatscale as intended + masterConfig.batteryConfig.vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI + masterConfig.batteryConfig.vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI + masterConfig.batteryConfig.vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert + break; + case BST_SET_MOTOR: + for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 + motor_disarmed[i] = bstRead16(); + break; + case BST_SET_SERVO_CONFIGURATION: #ifdef USE_SERVOS - if (bstReadDataSize() != 1 + sizeof(servoParam_t)) { - ret = BST_FAILED; - break; - } - i = bstRead8(); - if (i >= MAX_SUPPORTED_SERVOS) { - ret = BST_FAILED; - } else { - masterConfig.servoConf[i].min = bstRead16(); - masterConfig.servoConf[i].max = bstRead16(); - masterConfig.servoConf[i].middle = bstRead16(); - masterConfig.servoConf[i].rate = bstRead8(); - masterConfig.servoConf[i].angleAtMin = bstRead8(); - masterConfig.servoConf[i].angleAtMax = bstRead8(); - masterConfig.servoConf[i].forwardFromChannel = bstRead8(); - masterConfig.servoConf[i].reversedSources = bstRead32(); - } + if (bstReadDataSize() != 1 + sizeof(servoParam_t)) { + ret = BST_FAILED; + break; + } + i = bstRead8(); + if (i >= MAX_SUPPORTED_SERVOS) { + ret = BST_FAILED; + } else { + masterConfig.servoConf[i].min = bstRead16(); + masterConfig.servoConf[i].max = bstRead16(); + masterConfig.servoConf[i].middle = bstRead16(); + masterConfig.servoConf[i].rate = bstRead8(); + masterConfig.servoConf[i].angleAtMin = bstRead8(); + masterConfig.servoConf[i].angleAtMax = bstRead8(); + masterConfig.servoConf[i].forwardFromChannel = bstRead8(); + masterConfig.servoConf[i].reversedSources = bstRead32(); + } #endif - break; - case BST_SET_SERVO_MIX_RULE: + break; + case BST_SET_SERVO_MIX_RULE: #ifdef USE_SERVOS - i = bstRead8(); - if (i >= MAX_SERVO_RULES) { - ret = BST_FAILED; - } else { - masterConfig.customServoMixer[i].targetChannel = bstRead8(); - masterConfig.customServoMixer[i].inputSource = bstRead8(); - masterConfig.customServoMixer[i].rate = bstRead8(); - masterConfig.customServoMixer[i].speed = bstRead8(); - masterConfig.customServoMixer[i].min = bstRead8(); - masterConfig.customServoMixer[i].max = bstRead8(); - masterConfig.customServoMixer[i].box = bstRead8(); - loadCustomServoMixer(); - } + i = bstRead8(); + if (i >= MAX_SERVO_RULES) { + ret = BST_FAILED; + } else { + masterConfig.customServoMixer[i].targetChannel = bstRead8(); + masterConfig.customServoMixer[i].inputSource = bstRead8(); + masterConfig.customServoMixer[i].rate = bstRead8(); + masterConfig.customServoMixer[i].speed = bstRead8(); + masterConfig.customServoMixer[i].min = bstRead8(); + masterConfig.customServoMixer[i].max = bstRead8(); + masterConfig.customServoMixer[i].box = bstRead8(); + loadCustomServoMixer(); + } #endif - break; - case BST_RESET_CONF: - if (!ARMING_FLAG(ARMED)) { - resetEEPROM(); - readEEPROM(); - } - break; - case BST_ACC_CALIBRATION: - if (!ARMING_FLAG(ARMED)) - accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); - break; - case BST_MAG_CALIBRATION: - if (!ARMING_FLAG(ARMED)) - ENABLE_STATE(CALIBRATE_MAG); - break; - case BST_EEPROM_WRITE: - if (ARMING_FLAG(ARMED)) { - ret = BST_FAILED; - bstWrite8(ret); - //bstSlaveWrite(writeData); - return ret; - } - writeEEPROM(); - readEEPROM(); - break; + break; + case BST_RESET_CONF: + if (!ARMING_FLAG(ARMED)) { + resetEEPROM(); + readEEPROM(); + } + break; + case BST_ACC_CALIBRATION: + if (!ARMING_FLAG(ARMED)) + accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); + break; + case BST_MAG_CALIBRATION: + if (!ARMING_FLAG(ARMED)) + ENABLE_STATE(CALIBRATE_MAG); + break; + case BST_EEPROM_WRITE: + if (ARMING_FLAG(ARMED)) { + ret = BST_FAILED; + bstWrite8(ret); + //bstSlaveWrite(writeData); + return ret; + } + writeEEPROM(); + readEEPROM(); + break; #ifdef USE_FLASHFS - case BST_DATAFLASH_ERASE: - flashfsEraseCompletely(); - break; + case BST_DATAFLASH_ERASE: + flashfsEraseCompletely(); + break; #endif #ifdef GPS - case BST_SET_RAW_GPS: - if (bstRead8()) { - ENABLE_STATE(GPS_FIX); - } else { - DISABLE_STATE(GPS_FIX); - } - GPS_numSat = bstRead8(); - GPS_coord[LAT] = bstRead32(); - GPS_coord[LON] = bstRead32(); - GPS_altitude = bstRead16(); - GPS_speed = bstRead16(); - GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers - break; - case BST_SET_WP: - wp_no = bstRead8(); //get the wp number - lat = bstRead32(); - lon = bstRead32(); - alt = bstRead32(); // to set altitude (cm) - bstRead16(); // future: to set heading (deg) - bstRead16(); // future: to set time to stay (ms) - bstRead8(); // future: to set nav flag - if (wp_no == 0) { - GPS_home[LAT] = lat; - GPS_home[LON] = lon; - DISABLE_FLIGHT_MODE(GPS_HOME_MODE); // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS - ENABLE_STATE(GPS_FIX_HOME); - if (alt != 0) - AltHold = alt; // temporary implementation to test feature with apps - } else if (wp_no == 16) { // OK with SERIAL GPS -- NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS - GPS_hold[LAT] = lat; - GPS_hold[LON] = lon; - if (alt != 0) - AltHold = alt; // temporary implementation to test feature with apps - nav_mode = NAV_MODE_WP; - GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); - } - break; + case BST_SET_RAW_GPS: + if (bstRead8()) { + ENABLE_STATE(GPS_FIX); + } else { + DISABLE_STATE(GPS_FIX); + } + GPS_numSat = bstRead8(); + GPS_coord[LAT] = bstRead32(); + GPS_coord[LON] = bstRead32(); + GPS_altitude = bstRead16(); + GPS_speed = bstRead16(); + GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers + break; + case BST_SET_WP: + wp_no = bstRead8(); //get the wp number + lat = bstRead32(); + lon = bstRead32(); + alt = bstRead32(); // to set altitude (cm) + bstRead16(); // future: to set heading (deg) + bstRead16(); // future: to set time to stay (ms) + bstRead8(); // future: to set nav flag + if (wp_no == 0) { + GPS_home[LAT] = lat; + GPS_home[LON] = lon; + DISABLE_FLIGHT_MODE(GPS_HOME_MODE); // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS + ENABLE_STATE(GPS_FIX_HOME); + if (alt != 0) + AltHold = alt; // temporary implementation to test feature with apps + } else if (wp_no == 16) { // OK with SERIAL GPS -- NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS + GPS_hold[LAT] = lat; + GPS_hold[LON] = lon; + if (alt != 0) + AltHold = alt; // temporary implementation to test feature with apps + nav_mode = NAV_MODE_WP; + GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); + } + break; #endif - case BST_SET_FEATURE: - featureClearAll(); - featureSet(bstRead32()); // features bitmap - break; - case BST_SET_BOARD_ALIGNMENT: - masterConfig.boardAlignment.rollDegrees = bstRead16(); - masterConfig.boardAlignment.pitchDegrees = bstRead16(); - masterConfig.boardAlignment.yawDegrees = bstRead16(); - break; - case BST_SET_VOLTAGE_METER_CONFIG: - masterConfig.batteryConfig.vbatscale = bstRead8(); // actual vbatscale as intended - masterConfig.batteryConfig.vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI - masterConfig.batteryConfig.vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI - masterConfig.batteryConfig.vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert - break; - case BST_SET_CURRENT_METER_CONFIG: - masterConfig.batteryConfig.currentMeterScale = bstRead16(); - masterConfig.batteryConfig.currentMeterOffset = bstRead16(); - masterConfig.batteryConfig.currentMeterType = bstRead8(); - masterConfig.batteryConfig.batteryCapacity = bstRead16(); - break; + case BST_SET_FEATURE: + featureClearAll(); + featureSet(bstRead32()); // features bitmap + break; + case BST_SET_BOARD_ALIGNMENT: + masterConfig.boardAlignment.rollDegrees = bstRead16(); + masterConfig.boardAlignment.pitchDegrees = bstRead16(); + masterConfig.boardAlignment.yawDegrees = bstRead16(); + break; + case BST_SET_VOLTAGE_METER_CONFIG: + masterConfig.batteryConfig.vbatscale = bstRead8(); // actual vbatscale as intended + masterConfig.batteryConfig.vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI + masterConfig.batteryConfig.vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI + masterConfig.batteryConfig.vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert + break; + case BST_SET_CURRENT_METER_CONFIG: + masterConfig.batteryConfig.currentMeterScale = bstRead16(); + masterConfig.batteryConfig.currentMeterOffset = bstRead16(); + masterConfig.batteryConfig.currentMeterType = bstRead8(); + masterConfig.batteryConfig.batteryCapacity = bstRead16(); + break; #ifndef USE_QUAD_MIXER_ONLY - case BST_SET_MIXER: - masterConfig.mixerMode = bstRead8(); - break; + case BST_SET_MIXER: + masterConfig.mixerMode = bstRead8(); + break; #endif - case BST_SET_RX_CONFIG: - masterConfig.rxConfig.serialrx_provider = bstRead8(); - masterConfig.rxConfig.maxcheck = bstRead16(); - masterConfig.rxConfig.midrc = bstRead16(); - masterConfig.rxConfig.mincheck = bstRead16(); - masterConfig.rxConfig.spektrum_sat_bind = bstRead8(); - if (bstReadDataSize() > 8) { - masterConfig.rxConfig.rx_min_usec = bstRead16(); - masterConfig.rxConfig.rx_max_usec = bstRead16(); - } - break; - case BST_SET_FAILSAFE_CONFIG: - masterConfig.failsafeConfig.failsafe_delay = bstRead8(); - masterConfig.failsafeConfig.failsafe_off_delay = bstRead8(); - masterConfig.failsafeConfig.failsafe_throttle = bstRead16(); - break; - case BST_SET_RXFAIL_CONFIG: - { - uint8_t channelCount = bstReadDataSize() / 3; - if (channelCount > MAX_AUX_CHANNEL_COUNT) { - ret = BST_FAILED; - } else { - for (i = NON_AUX_CHANNEL_COUNT; i < channelCount; i++) { - masterConfig.rxConfig.failsafe_channel_configurations[i].mode = bstRead8(); - masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(bstRead16()); - } - } - } - break; - case BST_SET_RSSI_CONFIG: - masterConfig.rxConfig.rssi_channel = bstRead8(); - break; - case BST_SET_RX_MAP: - for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { - masterConfig.rxConfig.rcmap[i] = bstRead8(); - } - break; - case BST_SET_BF_CONFIG: + case BST_SET_RX_CONFIG: + masterConfig.rxConfig.serialrx_provider = bstRead8(); + masterConfig.rxConfig.maxcheck = bstRead16(); + masterConfig.rxConfig.midrc = bstRead16(); + masterConfig.rxConfig.mincheck = bstRead16(); + masterConfig.rxConfig.spektrum_sat_bind = bstRead8(); + if (bstReadDataSize() > 8) { + masterConfig.rxConfig.rx_min_usec = bstRead16(); + masterConfig.rxConfig.rx_max_usec = bstRead16(); + } + break; + case BST_SET_FAILSAFE_CONFIG: + masterConfig.failsafeConfig.failsafe_delay = bstRead8(); + masterConfig.failsafeConfig.failsafe_off_delay = bstRead8(); + masterConfig.failsafeConfig.failsafe_throttle = bstRead16(); + break; + case BST_SET_RXFAIL_CONFIG: + { + uint8_t channelCount = bstReadDataSize() / 3; + if (channelCount > MAX_AUX_CHANNEL_COUNT) { + ret = BST_FAILED; + } else { + for (i = NON_AUX_CHANNEL_COUNT; i < channelCount; i++) { + masterConfig.rxConfig.failsafe_channel_configurations[i].mode = bstRead8(); + masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(bstRead16()); + } + } + } + break; + case BST_SET_RSSI_CONFIG: + masterConfig.rxConfig.rssi_channel = bstRead8(); + break; + case BST_SET_RX_MAP: + for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { + masterConfig.rxConfig.rcmap[i] = bstRead8(); + } + break; + case BST_SET_BF_CONFIG: #ifdef USE_QUAD_MIXER_ONLY - bstRead8(); // mixerMode ignored + bstRead8(); // mixerMode ignored #else - masterConfig.mixerMode = bstRead8(); // mixerMode + masterConfig.mixerMode = bstRead8(); // mixerMode #endif - featureClearAll(); - featureSet(bstRead32()); // features bitmap + featureClearAll(); + featureSet(bstRead32()); // features bitmap - masterConfig.rxConfig.serialrx_provider = bstRead8(); // serialrx_type + masterConfig.rxConfig.serialrx_provider = bstRead8(); // serialrx_type - masterConfig.boardAlignment.rollDegrees = bstRead16(); // board_align_roll - masterConfig.boardAlignment.pitchDegrees = bstRead16(); // board_align_pitch - masterConfig.boardAlignment.yawDegrees = bstRead16(); // board_align_yaw + masterConfig.boardAlignment.rollDegrees = bstRead16(); // board_align_roll + masterConfig.boardAlignment.pitchDegrees = bstRead16(); // board_align_pitch + masterConfig.boardAlignment.yawDegrees = bstRead16(); // board_align_yaw - masterConfig.batteryConfig.currentMeterScale = bstRead16(); - masterConfig.batteryConfig.currentMeterOffset = bstRead16(); - break; - case BST_SET_CF_SERIAL_CONFIG: - { - uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4); - if (bstReadDataSize() % portConfigSize != 0) { - ret = BST_FAILED; - break; - } + masterConfig.batteryConfig.currentMeterScale = bstRead16(); + masterConfig.batteryConfig.currentMeterOffset = bstRead16(); + break; + case BST_SET_CF_SERIAL_CONFIG: + { + uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4); + if (bstReadDataSize() % portConfigSize != 0) { + ret = BST_FAILED; + break; + } - uint8_t remainingPortsInPacket = bstReadDataSize() / portConfigSize; + uint8_t remainingPortsInPacket = bstReadDataSize() / portConfigSize; - while (remainingPortsInPacket--) { - uint8_t identifier = bstRead8(); + while (remainingPortsInPacket--) { + uint8_t identifier = bstRead8(); - serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier); - if (!portConfig) { - ret = BST_FAILED; - break; - } + serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier); + if (!portConfig) { + ret = BST_FAILED; + break; + } - portConfig->identifier = identifier; - portConfig->functionMask = bstRead16(); - portConfig->msp_baudrateIndex = bstRead8(); - portConfig->gps_baudrateIndex = bstRead8(); - portConfig->telemetry_baudrateIndex = bstRead8(); - portConfig->blackbox_baudrateIndex = bstRead8(); - } - } - break; + portConfig->identifier = identifier; + portConfig->functionMask = bstRead16(); + portConfig->msp_baudrateIndex = bstRead8(); + portConfig->gps_baudrateIndex = bstRead8(); + portConfig->telemetry_baudrateIndex = bstRead8(); + portConfig->blackbox_baudrateIndex = bstRead8(); + } + } + break; #ifdef LED_STRIP - case BST_SET_LED_COLORS: - //for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { - { - i = bstRead8(); - hsvColor_t *color = &masterConfig.colors[i]; - color->h = bstRead16(); - color->s = bstRead8(); - color->v = bstRead8(); - } - break; - case BST_SET_LED_STRIP_CONFIG: - { - i = bstRead8(); - if (i >= MAX_LED_STRIP_LENGTH || bstReadDataSize() != (1 + 7)) { - ret = BST_FAILED; - break; - } - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - uint16_t mask; - // currently we're storing directions and functions in a uint16 (flags) - // the msp uses 2 x uint16_t to cater for future expansion - mask = bstRead16(); - ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; + case BST_SET_LED_COLORS: + //for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + { + i = bstRead8(); + hsvColor_t *color = &masterConfig.colors[i]; + color->h = bstRead16(); + color->s = bstRead8(); + color->v = bstRead8(); + } + break; + case BST_SET_LED_STRIP_CONFIG: + { + i = bstRead8(); + if (i >= MAX_LED_STRIP_LENGTH || bstReadDataSize() != (1 + 7)) { + ret = BST_FAILED; + break; + } + ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + uint16_t mask; + // currently we're storing directions and functions in a uint16 (flags) + // the msp uses 2 x uint16_t to cater for future expansion + mask = bstRead16(); + ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; - mask = bstRead16(); - ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; + mask = bstRead16(); + ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; - mask = bstRead8(); - ledConfig->xy = CALCULATE_LED_X(mask); + mask = bstRead8(); + ledConfig->xy = CALCULATE_LED_X(mask); - mask = bstRead8(); - ledConfig->xy |= CALCULATE_LED_Y(mask); + mask = bstRead8(); + ledConfig->xy |= CALCULATE_LED_Y(mask); - ledConfig->color = bstRead8(); + ledConfig->color = bstRead8(); - reevalulateLedConfig(); - } - break; + reevalulateLedConfig(); + } + break; #endif - case BST_REBOOT: - isRebootScheduled = true; - break; - case BST_DISARM: - if (ARMING_FLAG(ARMED)) - mwDisarm(); - ENABLE_ARMING_FLAG(PREVENT_ARMING); - break; - case BST_ENABLE_ARM: - DISABLE_ARMING_FLAG(PREVENT_ARMING); - break; - case BST_SET_DEADBAND: - masterConfig.rcControlsConfig.alt_hold_deadband = bstRead8(); - masterConfig.rcControlsConfig.alt_hold_fast_change = bstRead8(); - masterConfig.rcControlsConfig.deadband = bstRead8(); - masterConfig.rcControlsConfig.yaw_deadband = bstRead8(); - break; - case BST_SET_FC_FILTERS: - masterConfig.gyro_lpf = bstRead16(); - break; + case BST_REBOOT: + isRebootScheduled = true; + break; + case BST_DISARM: + if (ARMING_FLAG(ARMED)) + mwDisarm(); + ENABLE_ARMING_FLAG(PREVENT_ARMING); + break; + case BST_ENABLE_ARM: + DISABLE_ARMING_FLAG(PREVENT_ARMING); + break; + case BST_SET_DEADBAND: + masterConfig.rcControlsConfig.alt_hold_deadband = bstRead8(); + masterConfig.rcControlsConfig.alt_hold_fast_change = bstRead8(); + masterConfig.rcControlsConfig.deadband = bstRead8(); + masterConfig.rcControlsConfig.yaw_deadband = bstRead8(); + break; + case BST_SET_FC_FILTERS: + masterConfig.gyro_lpf = bstRead16(); + break; - default: - // we do not know how to handle the (valid) message, indicate error BST - ret = BST_FAILED; - } - bstWrite8(ret); - //bstSlaveWrite(writeData); - if(ret == BST_FAILED) - return false; - return true; + default: + // we do not know how to handle the (valid) message, indicate error BST + ret = BST_FAILED; + } + bstWrite8(ret); + //bstSlaveWrite(writeData); + if(ret == BST_FAILED) + return false; + return true; } static bool bstSlaveUSBCommandFeedback(/*uint8_t bstFeedback*/) { - bstWrite8(BST_USB_DEVICE_INFO_FRAME); //Sub CPU Device Info FRAME - bstWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); - bstWrite8(0x00); - bstWrite8(0x00); - bstWrite8(0x00); - bstWrite8(FC_VERSION_MAJOR); //Firmware ID - bstWrite8(FC_VERSION_MINOR); //Firmware ID - bstWrite8(0x00); - bstWrite8(0x00); - //bstSlaveWrite(writeData); - return true; + bstWrite8(BST_USB_DEVICE_INFO_FRAME); //Sub CPU Device Info FRAME + bstWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); + bstWrite8(0x00); + bstWrite8(0x00); + bstWrite8(0x00); + bstWrite8(FC_VERSION_MAJOR); //Firmware ID + bstWrite8(FC_VERSION_MINOR); //Firmware ID + bstWrite8(0x00); + bstWrite8(0x00); + //bstSlaveWrite(writeData); + return true; } /*************************************************************************************************/ -#define BST_RESET_TIME 1.2*1000*1000 //micro-seconds +#define BST_RESET_TIME 1.2*1000*1000 //micro-seconds uint32_t resetBstTimer = 0; bool needResetCheck = true; extern bool cleanflight_data_ready; void bstProcessInCommand(void) { - readBufferPointer = 2; - if(bstCurrentAddress() == CLEANFLIGHT_FC) { - if(bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) { - uint8_t i; - writeBufferPointer = 1; - cleanflight_data_ready = false; - for(i = 0; i < DATA_BUFFER_SIZE; i++) { - writeData[i] = 0; - } - switch (bstRead8()) { - case BST_USB_DEVICE_INFO_REQUEST: - bstRead8(); - if(bstSlaveUSBCommandFeedback(/*bstRead8()*/)) - coreProReady = true; - break; - case BST_READ_COMMANDS: - bstWrite8(BST_READ_COMMANDS); - bstSlaveProcessFeedbackCommand(bstRead8()); - break; - case BST_WRITE_COMMANDS: - bstWrite8(BST_WRITE_COMMANDS); - bstSlaveProcessWriteCommand(bstRead8()); - break; - default: - // we do not know how to handle the (valid) message, indicate error BST - break; - } - cleanflight_data_ready = true; - } - } else if(bstCurrentAddress() == 0x00) { - if(bstReadCRC() == CRC8 && bstRead8()==BST_GENERAL_HEARTBEAT) { - resetBstTimer = micros(); - needResetCheck = true; - } - } + readBufferPointer = 2; + if(bstCurrentAddress() == CLEANFLIGHT_FC) { + if(bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) { + uint8_t i; + writeBufferPointer = 1; + cleanflight_data_ready = false; + for(i = 0; i < DATA_BUFFER_SIZE; i++) { + writeData[i] = 0; + } + switch (bstRead8()) { + case BST_USB_DEVICE_INFO_REQUEST: + bstRead8(); + if(bstSlaveUSBCommandFeedback(/*bstRead8()*/)) + coreProReady = true; + break; + case BST_READ_COMMANDS: + bstWrite8(BST_READ_COMMANDS); + bstSlaveProcessFeedbackCommand(bstRead8()); + break; + case BST_WRITE_COMMANDS: + bstWrite8(BST_WRITE_COMMANDS); + bstSlaveProcessWriteCommand(bstRead8()); + break; + default: + // we do not know how to handle the (valid) message, indicate error BST + break; + } + cleanflight_data_ready = true; + } + } 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; - } - } + if(needResetCheck) { + uint32_t currentTimer = micros(); + if(currentTimer >= (resetBstTimer + BST_RESET_TIME)) + { + bstTimeoutUserCallback(); + needResetCheck = false; + } + } } /*************************************************************************************************/ @@ -1522,33 +1522,33 @@ static uint8_t sendCounter = 0; void taskBstMasterProcess(void) { - if(coreProReady) { - uint32_t now = micros(); - if(now >= next02hzUpdateAt_1 && !bstWriteBusy()) { - writeFCModeToBST(); - next02hzUpdateAt_1 = now + UPDATE_AT_02HZ; - } - if(now >= next20hzUpdateAt_1 && !bstWriteBusy()) { - if(sendCounter == 0) - writeRCChannelToBST(); - else if(sendCounter == 1) - writeRollPitchYawToBST(); - sendCounter++; - if(sendCounter > 1) - sendCounter = 0; - next20hzUpdateAt_1 = now + UPDATE_AT_20HZ; - } + if(coreProReady) { + uint32_t now = micros(); + if(now >= next02hzUpdateAt_1 && !bstWriteBusy()) { + writeFCModeToBST(); + next02hzUpdateAt_1 = now + UPDATE_AT_02HZ; + } + if(now >= next20hzUpdateAt_1 && !bstWriteBusy()) { + if(sendCounter == 0) + writeRCChannelToBST(); + else if(sendCounter == 1) + writeRollPitchYawToBST(); + sendCounter++; + if(sendCounter > 1) + sendCounter = 0; + next20hzUpdateAt_1 = now + UPDATE_AT_20HZ; + } - if(sensors(SENSOR_GPS) && !bstWriteBusy()) - writeGpsPositionPrameToBST(); - } - bstMasterWriteLoop(); - if (isRebootScheduled) { - stopMotors(); - handleOneshotFeatureChangeOnRestart(); - systemReset(); - } - resetBstChecker(); + if(sensors(SENSOR_GPS) && !bstWriteBusy()) + writeGpsPositionPrameToBST(); + } + bstMasterWriteLoop(); + if (isRebootScheduled) { + stopMotors(); + handleOneshotFeatureChangeOnRestart(); + systemReset(); + } + resetBstChecker(); } /*************************************************************************************************/ @@ -1556,29 +1556,29 @@ static uint8_t masterWriteBufferPointer; static uint8_t masterWriteData[DATA_BUFFER_SIZE]; static void bstMasterStartBuffer(uint8_t address) { - masterWriteData[0] = address; - masterWriteBufferPointer = 2; + masterWriteData[0] = address; + masterWriteBufferPointer = 2; } static void bstMasterWrite8(uint8_t data) { - masterWriteData[masterWriteBufferPointer++] = data; - masterWriteData[1] = masterWriteBufferPointer; + masterWriteData[masterWriteBufferPointer++] = data; + masterWriteData[1] = masterWriteBufferPointer; } static void bstMasterWrite16(uint16_t data) { - bstMasterWrite8((uint8_t)(data >> 8)); - bstMasterWrite8((uint8_t)(data >> 0)); + bstMasterWrite8((uint8_t)(data >> 8)); + bstMasterWrite8((uint8_t)(data >> 0)); } static void bstMasterWrite32(uint32_t data) { - bstMasterWrite16((uint8_t)(data >> 16)); - bstMasterWrite16((uint8_t)(data >> 0)); + bstMasterWrite16((uint8_t)(data >> 16)); + bstMasterWrite16((uint8_t)(data >> 0)); } /*************************************************************************************************/ -#define PUBLIC_ADDRESS 0x00 +#define PUBLIC_ADDRESS 0x00 static int32_t lat = 0; static int32_t lon = 0; @@ -1587,108 +1587,108 @@ static uint8_t numOfSat = 0; bool writeGpsPositionPrameToBST(void) { - if((lat != GPS_coord[LAT]) || (lon != GPS_coord[LON]) || (alt != GPS_altitude) || (numOfSat != GPS_numSat)) { - lat = GPS_coord[LAT]; - lon = GPS_coord[LON]; - alt = GPS_altitude; - numOfSat = GPS_numSat; - uint16_t speed = (GPS_speed * 9 / 25); - uint16_t gpsHeading = 0; - uint16_t altitude = 0; - gpsHeading = GPS_ground_course * 10; - altitude = alt * 10 + 1000; + if((lat != GPS_coord[LAT]) || (lon != GPS_coord[LON]) || (alt != GPS_altitude) || (numOfSat != GPS_numSat)) { + lat = GPS_coord[LAT]; + lon = GPS_coord[LON]; + alt = GPS_altitude; + numOfSat = GPS_numSat; + uint16_t speed = (GPS_speed * 9 / 25); + uint16_t gpsHeading = 0; + uint16_t altitude = 0; + gpsHeading = GPS_ground_course * 10; + altitude = alt * 10 + 1000; - bstMasterStartBuffer(PUBLIC_ADDRESS); - bstMasterWrite8(GPS_POSITION_FRAME_ID); - bstMasterWrite32(lat); - bstMasterWrite32(lon); - bstMasterWrite16(speed); - bstMasterWrite16(gpsHeading); - bstMasterWrite16(altitude); - bstMasterWrite8(numOfSat); - bstMasterWrite8(0x00); + bstMasterStartBuffer(PUBLIC_ADDRESS); + bstMasterWrite8(GPS_POSITION_FRAME_ID); + bstMasterWrite32(lat); + bstMasterWrite32(lon); + bstMasterWrite16(speed); + bstMasterWrite16(gpsHeading); + bstMasterWrite16(altitude); + bstMasterWrite8(numOfSat); + bstMasterWrite8(0x00); - return bstMasterWrite(masterWriteData); - } else - return false; + return bstMasterWrite(masterWriteData); + } else + return false; } bool writeRollPitchYawToBST(void) { - int16_t X = -attitude.values.pitch * (M_PIf / 1800.0f) * 10000; - int16_t Y = attitude.values.roll * (M_PIf / 1800.0f) * 10000; - int16_t Z = 0;//radiusHeading * 10000; + int16_t X = -attitude.values.pitch * (M_PIf / 1800.0f) * 10000; + int16_t Y = attitude.values.roll * (M_PIf / 1800.0f) * 10000; + int16_t Z = 0;//radiusHeading * 10000; - bstMasterStartBuffer(PUBLIC_ADDRESS); - bstMasterWrite8(FC_ATTITUDE_FRAME_ID); - bstMasterWrite16(X); - bstMasterWrite16(Y); - bstMasterWrite16(Z); + bstMasterStartBuffer(PUBLIC_ADDRESS); + bstMasterWrite8(FC_ATTITUDE_FRAME_ID); + bstMasterWrite16(X); + bstMasterWrite16(Y); + bstMasterWrite16(Z); - return bstMasterWrite(masterWriteData); + return bstMasterWrite(masterWriteData); } bool writeRCChannelToBST(void) { - uint8_t i = 0; - bstMasterStartBuffer(PUBLIC_ADDRESS); - bstMasterWrite8(RC_CHANNEL_FRAME_ID); - for(i = 0; i < (USABLE_TIMER_CHANNEL_COUNT-1); i++) { - bstMasterWrite16(rcData[i]); - } + uint8_t i = 0; + bstMasterStartBuffer(PUBLIC_ADDRESS); + bstMasterWrite8(RC_CHANNEL_FRAME_ID); + for(i = 0; i < (USABLE_TIMER_CHANNEL_COUNT-1); i++) { + bstMasterWrite16(rcData[i]); + } - return bstMasterWrite(masterWriteData); + return bstMasterWrite(masterWriteData); } bool writeFCModeToBST(void) { #ifdef CLEANFLIGHT_FULL_STATUS_SET - uint32_t tmp = 0; + uint32_t tmp = 0; tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE | IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON | IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO | IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG | IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG | IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE)) << BOXGPSHOME | IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD | IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE | - IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | - IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | - IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE; - bstMasterStartBuffer(PUBLIC_ADDRESS); - bstMasterWrite8(CLEANFLIGHT_MODE_FRAME_ID); - bstMasterWrite32(tmp); - bstMasterWrite16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE | + IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | + IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | + IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE; + bstMasterStartBuffer(PUBLIC_ADDRESS); + bstMasterWrite8(CLEANFLIGHT_MODE_FRAME_ID); + bstMasterWrite32(tmp); + bstMasterWrite16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); #else - uint8_t tmp = 0; - tmp = IS_ENABLED(ARMING_FLAG(ARMED)) | - IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << 1 | - IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << 2 | - IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << 3 | - IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << 4 | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << 5 | - IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << 6 | - IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << 7; + uint8_t tmp = 0; + tmp = IS_ENABLED(ARMING_FLAG(ARMED)) | + IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << 1 | + IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << 2 | + IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << 3 | + IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << 4 | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << 5 | + IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << 6 | + IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << 7; - bstMasterStartBuffer(PUBLIC_ADDRESS); - bstMasterWrite8(CLEANFLIGHT_MODE_FRAME_ID); - bstMasterWrite8(tmp); - bstMasterWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); + bstMasterStartBuffer(PUBLIC_ADDRESS); + bstMasterWrite8(CLEANFLIGHT_MODE_FRAME_ID); + bstMasterWrite8(tmp); + bstMasterWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); #endif - return bstMasterWrite(masterWriteData); + return bstMasterWrite(masterWriteData); } /*************************************************************************************************/