1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 14:25:16 +03:00

Whitespace tidy

This commit is contained in:
Martin Budden 2017-07-05 07:42:07 +01:00
parent f4850845df
commit fd7d770fb1
101 changed files with 448 additions and 448 deletions

View file

@ -31,7 +31,7 @@ void assertFailed2(const char * file, int line)
assertFailureLine = line; assertFailureLine = line;
} }
#if defined(USE_ASSERT_STOP) #if defined(USE_ASSERT_STOP)
while(1) { while (1) {
__asm("BKPT #0\n") ; // Break into the debugger __asm("BKPT #0\n") ; // Break into the debugger
} }
#endif #endif
@ -44,7 +44,7 @@ void assertFailed1(int line)
assertFailureLine = line; assertFailureLine = line;
} }
#if defined(USE_ASSERT_STOP) #if defined(USE_ASSERT_STOP)
while(1) { while (1) {
__asm("BKPT #0\n") ; // Break into the debugger __asm("BKPT #0\n") ; // Break into the debugger
} }
#endif #endif

View file

@ -326,8 +326,8 @@ void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count)
*/ */
void sensorCalibrationResetState(sensorCalibrationState_t * state) void sensorCalibrationResetState(sensorCalibrationState_t * state)
{ {
for(int i = 0; i < 4; i++){ for (int i = 0; i < 4; i++){
for(int j = 0; j < 4; j++){ for (int j = 0; j < 4; j++){
state->XtX[i][j] = 0; state->XtX[i][j] = 0;
} }

View file

@ -87,7 +87,7 @@ extern const uint8_t __pg_resetdata_end[];
#define PG_FOREACH_PROFILE(_name) \ #define PG_FOREACH_PROFILE(_name) \
PG_FOREACH(_name) \ PG_FOREACH(_name) \
if(pgIsSystem(_name)) \ if (pgIsSystem(_name)) \
continue; \ continue; \
else \ else \
/**/ /**/
@ -98,7 +98,7 @@ extern const uint8_t __pg_resetdata_end[];
do { \ do { \
extern const pgRegistry_t _name ##_Registry; \ extern const pgRegistry_t _name ##_Registry; \
pgResetCurrent(&_name ## _Registry); \ pgResetCurrent(&_name ## _Registry); \
} while(0) \ } while (0) \
/**/ /**/
// Declare system config // Declare system config

View file

@ -152,7 +152,7 @@ static void adcInstanceInit(ADCDevice adcDevice)
//HAL_CLEANINVALIDATECACHE((uint32_t*)&adcValues[adcDevice], configuredAdcChannels); //HAL_CLEANINVALIDATECACHE((uint32_t*)&adcValues[adcDevice], configuredAdcChannels);
/*##-4- Start the conversion process #######################################*/ /*##-4- Start the conversion process #######################################*/
if(HAL_ADC_Start_DMA(&adc->ADCHandle, (uint32_t*)&adcValues[adcDevice], adc->usedChannelCount) != HAL_OK) if (HAL_ADC_Start_DMA(&adc->ADCHandle, (uint32_t*)&adcValues[adcDevice], adc->usedChannelCount) != HAL_OK)
{ {
/* Start Conversation Error */ /* Start Conversation Error */
} }

View file

@ -153,7 +153,7 @@ static void bmp280_get_up(void)
//check if pressure and temperature readings are valid, otherwise use previous measurements from the moment //check if pressure and temperature readings are valid, otherwise use previous measurements from the moment
if(ack){ if (ack){
bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4)); bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4));
bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4)); bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4));

View file

@ -183,12 +183,12 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_,
HAL_StatusTypeDef status; HAL_StatusTypeDef status;
if(reg_ == 0xFF) if (reg_ == 0xFF)
status = HAL_I2C_Master_Transmit(&i2cHandle[device].Handle,addr_ << 1,data, len_, I2C_DEFAULT_TIMEOUT); status = HAL_I2C_Master_Transmit(&i2cHandle[device].Handle,addr_ << 1,data, len_, I2C_DEFAULT_TIMEOUT);
else else
status = HAL_I2C_Mem_Write(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,data, len_, I2C_DEFAULT_TIMEOUT); status = HAL_I2C_Mem_Write(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,data, len_, I2C_DEFAULT_TIMEOUT);
if(status != HAL_OK) if (status != HAL_OK)
return i2cHandleHardwareFailure(device); return i2cHandleHardwareFailure(device);
return true; return true;
@ -212,12 +212,12 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t
HAL_StatusTypeDef status; HAL_StatusTypeDef status;
if(reg_ == 0xFF) if (reg_ == 0xFF)
status = HAL_I2C_Master_Receive(&i2cHandle[device].Handle,addr_ << 1,buf, len, I2C_DEFAULT_TIMEOUT); status = HAL_I2C_Master_Receive(&i2cHandle[device].Handle,addr_ << 1,buf, len, I2C_DEFAULT_TIMEOUT);
else else
status = HAL_I2C_Mem_Read(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_DEFAULT_TIMEOUT); status = HAL_I2C_Mem_Read(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_DEFAULT_TIMEOUT);
if(status != HAL_OK) if (status != HAL_OK)
return i2cHandleHardwareFailure(device); return i2cHandleHardwareFailure(device);
return true; return true;

View file

@ -142,7 +142,7 @@ void spiInitDevice(SPIDevice device)
IOInit(IOGetByTag(spi->mosi), OWNER_SPI, RESOURCE_SPI_MOSI, device + 1); IOInit(IOGetByTag(spi->mosi), OWNER_SPI, RESOURCE_SPI_MOSI, device + 1);
#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7)
if(spi->sdcard == true) if (spi->sdcard == true)
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->af); IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->af);
else else
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->af); IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->af);
@ -255,7 +255,7 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in)
bool spiIsBusBusy(SPI_TypeDef *instance) bool spiIsBusBusy(SPI_TypeDef *instance)
{ {
SPIDevice device = spiDeviceByInstance(instance); SPIDevice device = spiDeviceByInstance(instance);
if(spiHardwareMap[device].hspi.State == HAL_SPI_STATE_BUSY) if (spiHardwareMap[device].hspi.State == HAL_SPI_STATE_BUSY)
return true; return true;
else else
return false; return false;
@ -268,11 +268,11 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len
#define SPI_DEFAULT_TIMEOUT 10 #define SPI_DEFAULT_TIMEOUT 10
if(!out) // Tx only if (!out) // Tx only
{ {
status = HAL_SPI_Transmit(&spiHardwareMap[device].hspi, (uint8_t *)in, len, SPI_DEFAULT_TIMEOUT); status = HAL_SPI_Transmit(&spiHardwareMap[device].hspi, (uint8_t *)in, len, SPI_DEFAULT_TIMEOUT);
} }
else if(!in) // Rx only else if (!in) // Rx only
{ {
status = HAL_SPI_Receive(&spiHardwareMap[device].hspi, out, len, SPI_DEFAULT_TIMEOUT); status = HAL_SPI_Receive(&spiHardwareMap[device].hspi, out, len, SPI_DEFAULT_TIMEOUT);
} }
@ -281,7 +281,7 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len
status = HAL_SPI_TransmitReceive(&spiHardwareMap[device].hspi, (uint8_t *)in, out, len, SPI_DEFAULT_TIMEOUT); status = HAL_SPI_TransmitReceive(&spiHardwareMap[device].hspi, (uint8_t *)in, out, len, SPI_DEFAULT_TIMEOUT);
} }
if( status != HAL_OK) if ( status != HAL_OK)
spiTimeoutUserCallback(instance); spiTimeoutUserCallback(instance);
return true; return true;

View file

@ -70,7 +70,7 @@ void softSpiInit(const softSPIDevice_t *dev)
uint8_t softSpiTransferByte(const softSPIDevice_t *dev, uint8_t byte) uint8_t softSpiTransferByte(const softSPIDevice_t *dev, uint8_t byte)
{ {
for(int ii = 0; ii < 8; ++ii) { for (int ii = 0; ii < 8; ++ii) {
if (byte & 0x80) { if (byte & 0x80) {
IOHi(IOGetByTag(dev->mosiTag)); IOHi(IOGetByTag(dev->mosiTag));
} else { } else {

View file

@ -193,7 +193,7 @@ void i2c_OLED_clear_display(void)
i2c_OLED_send_cmd(0x40); // Display start line register to 0 i2c_OLED_send_cmd(0x40); // Display start line register to 0
i2c_OLED_send_cmd(0); // Set low col address to 0 i2c_OLED_send_cmd(0); // Set low col address to 0
i2c_OLED_send_cmd(0x10); // Set high col address to 0 i2c_OLED_send_cmd(0x10); // Set high col address to 0
for(uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture for (uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture
i2c_OLED_send_byte(0x00); // clear i2c_OLED_send_byte(0x00); // clear
} }
i2c_OLED_send_cmd(0x81); // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction i2c_OLED_send_cmd(0x81); // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction
@ -207,7 +207,7 @@ void i2c_OLED_clear_display_quick(void)
i2c_OLED_send_cmd(0x40); // Display start line register to 0 i2c_OLED_send_cmd(0x40); // Display start line register to 0
i2c_OLED_send_cmd(0); // Set low col address to 0 i2c_OLED_send_cmd(0); // Set low col address to 0
i2c_OLED_send_cmd(0x10); // Set high col address to 0 i2c_OLED_send_cmd(0x10); // Set high col address to 0
for(uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture for (uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture
i2c_OLED_send_byte(0x00); // clear i2c_OLED_send_byte(0x00); // clear
} }
} }

View file

@ -63,7 +63,7 @@ typedef enum {
dmaDescriptors[i].irqHandlerCallback(&dmaDescriptors[i]);\ dmaDescriptors[i].irqHandlerCallback(&dmaDescriptors[i]);\
} }
#define DMA_CLEAR_FLAG(d, flag) if(d->flagsShift > 31) d->dma->HIFCR = (flag << (d->flagsShift - 32)); else d->dma->LIFCR = (flag << d->flagsShift) #define DMA_CLEAR_FLAG(d, flag) if (d->flagsShift > 31) d->dma->HIFCR = (flag << (d->flagsShift - 32)); else d->dma->LIFCR = (flag << d->flagsShift)
#define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? d->dma->HISR & (flag << (d->flagsShift - 32)): d->dma->LISR & (flag << d->flagsShift)) #define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? d->dma->HISR & (flag << (d->flagsShift - 32)): d->dma->LISR & (flag << d->flagsShift))

View file

@ -76,7 +76,7 @@ static void enableDmaClock(uint32_t rcc)
/* Delay after an RCC peripheral clock enabling */ /* Delay after an RCC peripheral clock enabling */
tmpreg = READ_BIT(RCC->AHB1ENR, rcc); tmpreg = READ_BIT(RCC->AHB1ENR, rcc);
UNUSED(tmpreg); UNUSED(tmpreg);
} while(0); } while (0);
} }
void dmaInit(dmaHandlerIdentifier_e identifier, resourceOwner_t owner, uint8_t resourceIndex) void dmaInit(dmaHandlerIdentifier_e identifier, resourceOwner_t owner, uint8_t resourceIndex)
@ -97,7 +97,7 @@ void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr
/* Delay after an RCC peripheral clock enabling */ /* Delay after an RCC peripheral clock enabling */
tmpreg = READ_BIT(RCC->AHB1ENR, dmaDescriptors[identifier].rcc); tmpreg = READ_BIT(RCC->AHB1ENR, dmaDescriptors[identifier].rcc);
UNUSED(tmpreg); UNUSED(tmpreg);
} while(0); } while (0);
dmaDescriptors[identifier].irqHandlerCallback = callback; dmaDescriptors[identifier].irqHandlerCallback = callback;
dmaDescriptors[identifier].userParam = userParam; dmaDescriptors[identifier].userParam = userParam;

View file

@ -73,7 +73,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t conf
(void)config; (void)config;
int chIdx; int chIdx;
chIdx = IO_GPIOPinIdx(io); chIdx = IO_GPIOPinIdx(io);
if(chIdx < 0) if (chIdx < 0)
return; return;
extiChannelRec_t *rec = &extiChannelRecs[chIdx]; extiChannelRec_t *rec = &extiChannelRecs[chIdx];
int group = extiGroups[chIdx]; int group = extiGroups[chIdx];
@ -91,7 +91,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t conf
//EXTI_ClearITPendingBit(extiLine); //EXTI_ClearITPendingBit(extiLine);
if(extiGroupPriority[group] > irqPriority) { if (extiGroupPriority[group] > irqPriority) {
extiGroupPriority[group] = irqPriority; extiGroupPriority[group] = irqPriority;
HAL_NVIC_SetPriority(extiGroupIRQn[group], NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority)); HAL_NVIC_SetPriority(extiGroupIRQn[group], NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
HAL_NVIC_EnableIRQ(extiGroupIRQn[group]); HAL_NVIC_EnableIRQ(extiGroupIRQn[group]);
@ -103,7 +103,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_Typ
{ {
int chIdx; int chIdx;
chIdx = IO_GPIOPinIdx(io); chIdx = IO_GPIOPinIdx(io);
if(chIdx < 0) if (chIdx < 0)
return; return;
// we have only 16 extiChannelRecs // we have only 16 extiChannelRecs
@ -133,7 +133,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_Typ
EXTIInit.EXTI_LineCmd = ENABLE; EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit); EXTI_Init(&EXTIInit);
if(extiGroupPriority[group] > irqPriority) { if (extiGroupPriority[group] > irqPriority) {
extiGroupPriority[group] = irqPriority; extiGroupPriority[group] = irqPriority;
NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitTypeDef NVIC_InitStructure;
@ -153,7 +153,7 @@ void EXTIRelease(IO_t io)
int chIdx; int chIdx;
chIdx = IO_GPIOPinIdx(io); chIdx = IO_GPIOPinIdx(io);
if(chIdx < 0) if (chIdx < 0)
return; return;
// we have only 16 extiChannelRecs // we have only 16 extiChannelRecs
@ -167,18 +167,18 @@ void EXTIEnable(IO_t io, bool enable)
{ {
#if defined(STM32F1) || defined(STM32F4) || defined(STM32F7) #if defined(STM32F1) || defined(STM32F4) || defined(STM32F7)
uint32_t extiLine = IO_EXTI_Line(io); uint32_t extiLine = IO_EXTI_Line(io);
if(!extiLine) if (!extiLine)
return; return;
if(enable) if (enable)
EXTI->IMR |= extiLine; EXTI->IMR |= extiLine;
else else
EXTI->IMR &= ~extiLine; EXTI->IMR &= ~extiLine;
#elif defined(STM32F303xC) #elif defined(STM32F303xC)
int extiLine = IO_EXTI_Line(io); int extiLine = IO_EXTI_Line(io);
if(extiLine < 0) if (extiLine < 0)
return; return;
// assume extiLine < 32 (valid for all EXTI pins) // assume extiLine < 32 (valid for all EXTI pins)
if(enable) if (enable)
EXTI->IMR |= 1 << extiLine; EXTI->IMR |= 1 << extiLine;
else else
EXTI->IMR &= ~(1 << extiLine); EXTI->IMR &= ~(1 << extiLine);
@ -191,7 +191,7 @@ void EXTI_IRQHandler(void)
{ {
uint32_t exti_active = EXTI->IMR & EXTI->PR; uint32_t exti_active = EXTI->IMR & EXTI->PR;
while(exti_active) { while (exti_active) {
unsigned idx = 31 - __builtin_clz(exti_active); unsigned idx = 31 - __builtin_clz(exti_active);
uint32_t mask = 1 << idx; uint32_t mask = 1 << idx;
extiChannelRecs[idx].handler->fn(extiChannelRecs[idx].handler); extiChannelRecs[idx].handler->fn(extiChannelRecs[idx].handler);

View file

@ -25,9 +25,9 @@
# define LED0_OFF ledSet(0, false) # define LED0_OFF ledSet(0, false)
# define LED0_ON ledSet(0, true) # define LED0_ON ledSet(0, true)
#else #else
# define LED0_TOGGLE do {} while(0) # define LED0_TOGGLE do {} while (0)
# define LED0_OFF do {} while(0) # define LED0_OFF do {} while (0)
# define LED0_ON do {} while(0) # define LED0_ON do {} while (0)
#endif #endif
#ifdef LED1 #ifdef LED1
@ -35,9 +35,9 @@
# define LED1_OFF ledSet(1, false) # define LED1_OFF ledSet(1, false)
# define LED1_ON ledSet(1, true) # define LED1_ON ledSet(1, true)
#else #else
# define LED1_TOGGLE do {} while(0) # define LED1_TOGGLE do {} while (0)
# define LED1_OFF do {} while(0) # define LED1_OFF do {} while (0)
# define LED1_ON do {} while(0) # define LED1_ON do {} while (0)
#endif #endif
#ifdef LED2 #ifdef LED2
@ -45,9 +45,9 @@
# define LED2_OFF ledSet(2, false) # define LED2_OFF ledSet(2, false)
# define LED2_ON ledSet(2, true) # define LED2_ON ledSet(2, true)
#else #else
# define LED2_TOGGLE do {} while(0) # define LED2_TOGGLE do {} while (0)
# define LED2_OFF do {} while(0) # define LED2_OFF do {} while (0)
# define LED2_ON do {} while(0) # define LED2_ON do {} while (0)
#endif #endif
void ledInit(bool alternative_led); void ledInit(bool alternative_led);

View file

@ -50,7 +50,7 @@ static TIM_HandleTypeDef TimHandle;
void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim)
{ {
if(htim->Instance==WS2811_TIMER) if (htim->Instance==WS2811_TIMER)
{ {
//HAL_TIM_PWM_Stop_DMA(&TimHandle,WS2811_TIMER_CHANNEL); //HAL_TIM_PWM_Stop_DMA(&TimHandle,WS2811_TIMER_CHANNEL);
ws2811LedDataTransferInProgress = 0; ws2811LedDataTransferInProgress = 0;
@ -117,7 +117,7 @@ void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef *htim)
dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, timDMASource); dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, timDMASource);
/* Initialize TIMx DMA handle */ /* Initialize TIMx DMA handle */
if(HAL_DMA_Init(htim->hdma[timDMASource]) != HAL_OK) if (HAL_DMA_Init(htim->hdma[timDMASource]) != HAL_OK)
{ {
/* Initialization Error */ /* Initialization Error */
return; return;
@ -132,7 +132,7 @@ void ws2811LedStripHardwareInit(void)
TimHandle.Init.Period = 135; // 800kHz TimHandle.Init.Period = 135; // 800kHz
TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
if(HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) if (HAL_TIM_PWM_Init(&TimHandle) != HAL_OK)
{ {
/* Initialization Error */ /* Initialization Error */
return; return;
@ -148,7 +148,7 @@ void ws2811LedStripHardwareInit(void)
TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
if(HAL_TIM_PWM_ConfigChannel(&TimHandle, &TIM_OCInitStructure, WS2811_TIMER_CHANNEL) != HAL_OK) if (HAL_TIM_PWM_ConfigChannel(&TimHandle, &TIM_OCInitStructure, WS2811_TIMER_CHANNEL) != HAL_OK)
{ {
/* Configuration Error */ /* Configuration Error */
return; return;
@ -168,7 +168,7 @@ void ws2811LedStripDMAEnable(void)
return; return;
} }
if( HAL_TIM_PWM_Start_DMA(&TimHandle, WS2811_TIMER_CHANNEL, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) if ( HAL_TIM_PWM_Start_DMA(&TimHandle, WS2811_TIMER_CHANNEL, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK)
{ {
/* Starting PWM generation Error */ /* Starting PWM generation Error */
ws2811LedDataTransferInProgress = 0; ws2811LedDataTransferInProgress = 0;

View file

@ -213,7 +213,7 @@ void max7456ReInit(void)
ENABLE_MAX7456; ENABLE_MAX7456;
switch(videoSignalCfg) { switch (videoSignalCfg) {
case PAL: case PAL:
videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE; videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE;
break; break;
@ -235,7 +235,7 @@ void max7456ReInit(void)
} }
// set all rows to same charactor black/white level // set all rows to same charactor black/white level
for(x = 0; x < maxScreenRows; x++) { for (x = 0; x < maxScreenRows; x++) {
max7456Send(MAX7456ADD_RB0 + x, BWBRIGHTNESS); max7456Send(MAX7456ADD_RB0 + x, BWBRIGHTNESS);
} }
@ -404,7 +404,7 @@ void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
max7456Send(MAX7456ADD_CMAH, char_address); // set start address high max7456Send(MAX7456ADD_CMAH, char_address); // set start address high
for(x = 0; x < 54; x++) { for (x = 0; x < 54; x++) {
max7456Send(MAX7456ADD_CMAL, x); //set start address low max7456Send(MAX7456ADD_CMAL, x); //set start address low
max7456Send(MAX7456ADD_CMDI, font_data[x]); max7456Send(MAX7456ADD_CMDI, font_data[x]);
#ifdef LED0_TOGGLE #ifdef LED0_TOGGLE

View file

@ -67,7 +67,7 @@ static void ms4525_start(void)
static void ms4525_read(void) static void ms4525_read(void)
{ {
if(i2cRead( PITOT_I2C_INSTANCE, MS4525_ADDR, 0xFF, 4, rxbuf )) { if (i2cRead( PITOT_I2C_INSTANCE, MS4525_ADDR, 0xFF, 4, rxbuf )) {
ms4525_up = (rxbuf[0] << 8) | (rxbuf[1] << 0); ms4525_up = (rxbuf[0] << 8) | (rxbuf[1] << 0);
ms4525_ut = ((rxbuf[2] << 8) | (rxbuf[3] << 0))>>5; ms4525_ut = ((rxbuf[2] << 8) | (rxbuf[3] << 0))>>5;
} }

View file

@ -68,7 +68,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
{ {
#if defined(USE_HAL_DRIVER) #if defined(USE_HAL_DRIVER)
TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim); TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
if(Handle == NULL) return; if (Handle == NULL) return;
TIM_OC_InitTypeDef TIM_OCInitStructure; TIM_OC_InitTypeDef TIM_OCInitStructure;
@ -116,7 +116,7 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8
pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++]; pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++];
#if defined(USE_HAL_DRIVER) #if defined(USE_HAL_DRIVER)
TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim); TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
if(Handle == NULL) return p; if (Handle == NULL) return p;
#endif #endif
configTimeBase(timerHardware->tim, period, mhz); configTimeBase(timerHardware->tim, period, mhz);
@ -142,7 +142,7 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8
pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output & TIMER_OUTPUT_INVERTED); pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output & TIMER_OUTPUT_INVERTED);
#if defined(USE_HAL_DRIVER) #if defined(USE_HAL_DRIVER)
if(timerHardware->output & TIMER_OUTPUT_N_CHANNEL) if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL)
HAL_TIMEx_PWMN_Start(Handle, timerHardware->channel); HAL_TIMEx_PWMN_Start(Handle, timerHardware->channel);
else else
HAL_TIM_PWM_Start(Handle, timerHardware->channel); HAL_TIM_PWM_Start(Handle, timerHardware->channel);
@ -310,10 +310,10 @@ void pwmWriteServo(uint8_t index, uint16_t value)
#ifdef BEEPER_PWM #ifdef BEEPER_PWM
void pwmWriteBeeper(bool onoffBeep) void pwmWriteBeeper(bool onoffBeep)
{ {
if(beeperPwm == NULL) if (beeperPwm == NULL)
return; return;
if(onoffBeep == true) { if (onoffBeep == true) {
*beeperPwm->ccr = (1000000 / beeperFrequency) / 2; *beeperPwm->ccr = (1000000 / beeperFrequency) / 2;
} else { } else {
*beeperPwm->ccr = 0; *beeperPwm->ccr = 0;

View file

@ -356,7 +356,7 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
{ {
TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim); TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
if(Handle == NULL) return; if (Handle == NULL) return;
TIM_IC_InitTypeDef TIM_ICInitStructure; TIM_IC_InitTypeDef TIM_ICInitStructure;

View file

@ -92,7 +92,7 @@ static void uartReconfigure(uartPort_t *uartPort)
usartConfigurePinInversion(uartPort); usartConfigurePinInversion(uartPort);
if(uartPort->port.options & SERIAL_BIDIR) if (uartPort->port.options & SERIAL_BIDIR)
USART_HalfDuplexCmd(uartPort->USARTx, ENABLE); USART_HalfDuplexCmd(uartPort->USARTx, ENABLE);
else else
USART_HalfDuplexCmd(uartPort->USARTx, DISABLE); USART_HalfDuplexCmd(uartPort->USARTx, DISABLE);

View file

@ -41,7 +41,7 @@
static void usartConfigurePinInversion(uartPort_t *uartPort) { static void usartConfigurePinInversion(uartPort_t *uartPort) {
bool inverted = uartPort->port.options & SERIAL_INVERTED; bool inverted = uartPort->port.options & SERIAL_INVERTED;
if(inverted) if (inverted)
{ {
if (uartPort->port.mode & MODE_RX) if (uartPort->port.mode & MODE_RX)
{ {
@ -88,7 +88,7 @@ static void uartReconfigure(uartPort_t *uartPort)
usartConfigurePinInversion(uartPort); usartConfigurePinInversion(uartPort);
if(uartPort->port.options & SERIAL_BIDIR) if (uartPort->port.options & SERIAL_BIDIR)
{ {
HAL_HalfDuplex_Init(&uartPort->Handle); HAL_HalfDuplex_Init(&uartPort->Handle);
} }
@ -161,9 +161,9 @@ static void uartReconfigure(uartPort_t *uartPort)
HAL_DMA_DeInit(&uartPort->txDMAHandle); HAL_DMA_DeInit(&uartPort->txDMAHandle);
HAL_StatusTypeDef status = HAL_DMA_Init(&uartPort->txDMAHandle); HAL_StatusTypeDef status = HAL_DMA_Init(&uartPort->txDMAHandle);
if(status != HAL_OK) if (status != HAL_OK)
{ {
while(1); while (1);
} }
/* Associate the initialized DMA handle to the UART handle */ /* Associate the initialized DMA handle to the UART handle */
__HAL_LINKDMA(&uartPort->Handle, hdmatx, uartPort->txDMAHandle); __HAL_LINKDMA(&uartPort->Handle, hdmatx, uartPort->txDMAHandle);
@ -253,7 +253,7 @@ void uartStartTxDMA(uartPort_t *s)
uint16_t size = 0; uint16_t size = 0;
uint32_t fromwhere=0; uint32_t fromwhere=0;
HAL_UART_StateTypeDef state = HAL_UART_GetState(&s->Handle); HAL_UART_StateTypeDef state = HAL_UART_GetState(&s->Handle);
if((state & HAL_UART_STATE_BUSY_TX) == HAL_UART_STATE_BUSY_TX) if ((state & HAL_UART_STATE_BUSY_TX) == HAL_UART_STATE_BUSY_TX)
return; return;
if (s->port.txBufferHead > s->port.txBufferTail) { if (s->port.txBufferHead > s->port.txBufferTail) {

View file

@ -299,7 +299,7 @@ void uartIrqHandler(uartPort_t *s)
{ {
UART_HandleTypeDef *huart = &s->Handle; UART_HandleTypeDef *huart = &s->Handle;
/* UART in mode Receiver ---------------------------------------------------*/ /* UART in mode Receiver ---------------------------------------------------*/
if((__HAL_UART_GET_IT(huart, UART_IT_RXNE) != RESET)) if ((__HAL_UART_GET_IT(huart, UART_IT_RXNE) != RESET))
{ {
uint8_t rbyte = (uint8_t)(huart->Instance->RDR & (uint8_t)0xff); uint8_t rbyte = (uint8_t)(huart->Instance->RDR & (uint8_t)0xff);
@ -318,37 +318,37 @@ void uartIrqHandler(uartPort_t *s)
} }
/* UART parity error interrupt occurred -------------------------------------*/ /* UART parity error interrupt occurred -------------------------------------*/
if((__HAL_UART_GET_IT(huart, UART_IT_PE) != RESET)) if ((__HAL_UART_GET_IT(huart, UART_IT_PE) != RESET))
{ {
__HAL_UART_CLEAR_IT(huart, UART_CLEAR_PEF); __HAL_UART_CLEAR_IT(huart, UART_CLEAR_PEF);
} }
/* UART frame error interrupt occurred --------------------------------------*/ /* UART frame error interrupt occurred --------------------------------------*/
if((__HAL_UART_GET_IT(huart, UART_IT_FE) != RESET)) if ((__HAL_UART_GET_IT(huart, UART_IT_FE) != RESET))
{ {
__HAL_UART_CLEAR_IT(huart, UART_CLEAR_FEF); __HAL_UART_CLEAR_IT(huart, UART_CLEAR_FEF);
} }
/* UART noise error interrupt occurred --------------------------------------*/ /* UART noise error interrupt occurred --------------------------------------*/
if((__HAL_UART_GET_IT(huart, UART_IT_NE) != RESET)) if ((__HAL_UART_GET_IT(huart, UART_IT_NE) != RESET))
{ {
__HAL_UART_CLEAR_IT(huart, UART_CLEAR_NEF); __HAL_UART_CLEAR_IT(huart, UART_CLEAR_NEF);
} }
/* UART Over-Run interrupt occurred -----------------------------------------*/ /* UART Over-Run interrupt occurred -----------------------------------------*/
if((__HAL_UART_GET_IT(huart, UART_IT_ORE) != RESET)) if ((__HAL_UART_GET_IT(huart, UART_IT_ORE) != RESET))
{ {
__HAL_UART_CLEAR_IT(huart, UART_CLEAR_OREF); __HAL_UART_CLEAR_IT(huart, UART_CLEAR_OREF);
} }
/* UART in mode Transmitter ------------------------------------------------*/ /* UART in mode Transmitter ------------------------------------------------*/
if((__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET)) if ((__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET))
{ {
HAL_UART_IRQHandler(huart); HAL_UART_IRQHandler(huart);
} }
/* UART in mode Transmitter (transmission end) -----------------------------*/ /* UART in mode Transmitter (transmission end) -----------------------------*/
if((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET)) if ((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET))
{ {
HAL_UART_IRQHandler(huart); HAL_UART_IRQHandler(huart);
handleUsartTxDma(s); handleUsartTxDma(s);
@ -428,7 +428,7 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
//HAL_NVIC_SetPriority(uart->txIrq, NVIC_PRIORITY_BASE(uart->txPriority), NVIC_PRIORITY_SUB(uart->txPriority)); //HAL_NVIC_SetPriority(uart->txIrq, NVIC_PRIORITY_BASE(uart->txPriority), NVIC_PRIORITY_SUB(uart->txPriority));
//HAL_NVIC_EnableIRQ(uart->txIrq); //HAL_NVIC_EnableIRQ(uart->txIrq);
if(!s->rxDMAChannel) if (!s->rxDMAChannel)
{ {
HAL_NVIC_SetPriority(uart->rxIrq, NVIC_PRIORITY_BASE(uart->rxPriority), NVIC_PRIORITY_SUB(uart->rxPriority)); HAL_NVIC_SetPriority(uart->rxIrq, NVIC_PRIORITY_BASE(uart->rxPriority), NVIC_PRIORITY_SUB(uart->rxPriority));
HAL_NVIC_EnableIRQ(uart->rxIrq); HAL_NVIC_EnableIRQ(uart->rxIrq);

View file

@ -24,9 +24,9 @@
#define BEEP_OFF systemBeep(false) #define BEEP_OFF systemBeep(false)
#define BEEP_ON systemBeep(true) #define BEEP_ON systemBeep(true)
#else #else
#define BEEP_TOGGLE do {} while(0) #define BEEP_TOGGLE do {} while (0)
#define BEEP_OFF do {} while(0) #define BEEP_OFF do {} while (0)
#define BEEP_ON do {} while(0) #define BEEP_ON do {} while (0)
#endif #endif
typedef struct beeperDevConfig_s { typedef struct beeperDevConfig_s {

View file

@ -78,7 +78,7 @@ static uint8_t lookupTimerIndex(const TIM_TypeDef *tim)
#define _CASE(i) _CASE_(TIM##i##_BASE, TIMER_INDEX(i)) #define _CASE(i) _CASE_(TIM##i##_BASE, TIMER_INDEX(i))
// let gcc do the work, switch should be quite optimized // let gcc do the work, switch should be quite optimized
switch((unsigned)tim >> _CASE_SHF) { switch ((unsigned)tim >> _CASE_SHF) {
#if USED_TIMERS & TIM_N(1) #if USED_TIMERS & TIM_N(1)
_CASE(1); _CASE(1);
#endif #endif
@ -254,7 +254,7 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui
timerNVICConfigure(irq, NVIC_PRIO_TIMER); timerNVICConfigure(irq, NVIC_PRIO_TIMER);
// HACK - enable second IRQ on timers that need it // HACK - enable second IRQ on timers that need it
switch(irq) { switch (irq) {
#if defined(STM32F10X) #if defined(STM32F10X)
case TIM1_CC_IRQn: case TIM1_CC_IRQn:
timerNVICConfigure(TIM1_UP_IRQn, NVIC_PRIO_TIMER); timerNVICConfigure(TIM1_UP_IRQn, NVIC_PRIO_TIMER);
@ -287,14 +287,14 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui
void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority, uint8_t irq) void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority, uint8_t irq)
{ {
unsigned channel = timHw - timerHardware; unsigned channel = timHw - timerHardware;
if(channel >= USABLE_TIMER_CHANNEL_COUNT) if (channel >= USABLE_TIMER_CHANNEL_COUNT)
return; return;
timerChannelInfo[channel].type = type; timerChannelInfo[channel].type = type;
unsigned timer = lookupTimerIndex(timHw->tim); unsigned timer = lookupTimerIndex(timHw->tim);
if(timer >= USED_TIMER_COUNT) if (timer >= USED_TIMER_COUNT)
return; return;
if(irqPriority < timerInfo[timer].priority) { if (irqPriority < timerInfo[timer].priority) {
// it would be better to set priority in the end, but current startup sequence is not ready // it would be better to set priority in the end, but current startup sequence is not ready
configTimeBase(usedTimers[timer], 0, 1); configTimeBase(usedTimers[timer], 0, 1);
TIM_Cmd(usedTimers[timer], ENABLE); TIM_Cmd(usedTimers[timer], ENABLE);
@ -321,8 +321,8 @@ void timerChOvrHandlerInit(timerOvrHandlerRec_t *self, timerOvrHandlerCallback *
static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, TIM_TypeDef *tim) { static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, TIM_TypeDef *tim) {
timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive; timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive;
ATOMIC_BLOCK(NVIC_PRIO_TIMER) { ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
for(int i = 0; i < CC_CHANNELS_PER_TIMER; i++) for (int i = 0; i < CC_CHANNELS_PER_TIMER; i++)
if(cfg->overflowCallback[i]) { if (cfg->overflowCallback[i]) {
*chain = cfg->overflowCallback[i]; *chain = cfg->overflowCallback[i];
chain = &cfg->overflowCallback[i]->next; chain = &cfg->overflowCallback[i]->next;
} }
@ -340,13 +340,13 @@ void timerChConfigCallbacks(const timerHardware_t *timHw, timerCCHandlerRec_t *e
return; return;
} }
uint8_t channelIndex = lookupChannelIndex(timHw->channel); uint8_t channelIndex = lookupChannelIndex(timHw->channel);
if(edgeCallback == NULL) // disable irq before changing callback to NULL if (edgeCallback == NULL) // disable irq before changing callback to NULL
TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel), DISABLE); TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel), DISABLE);
// setup callback info // setup callback info
timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallback; timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallback;
timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback; timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback;
// enable channel IRQ // enable channel IRQ
if(edgeCallback) if (edgeCallback)
TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel), ENABLE); TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel), ENABLE);
timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim); timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim);
@ -365,9 +365,9 @@ void timerChConfigCallbacksDual(const timerHardware_t *timHw, timerCCHandlerRec_
uint16_t chHi = timHw->channel | TIM_Channel_2; // upper channel uint16_t chHi = timHw->channel | TIM_Channel_2; // upper channel
uint8_t channelIndex = lookupChannelIndex(chLo); // get index of lower channel uint8_t channelIndex = lookupChannelIndex(chLo); // get index of lower channel
if(edgeCallbackLo == NULL) // disable irq before changing setting callback to NULL if (edgeCallbackLo == NULL) // disable irq before changing setting callback to NULL
TIM_ITConfig(timHw->tim, TIM_IT_CCx(chLo), DISABLE); TIM_ITConfig(timHw->tim, TIM_IT_CCx(chLo), DISABLE);
if(edgeCallbackHi == NULL) // disable irq before changing setting callback to NULL if (edgeCallbackHi == NULL) // disable irq before changing setting callback to NULL
TIM_ITConfig(timHw->tim, TIM_IT_CCx(chHi), DISABLE); TIM_ITConfig(timHw->tim, TIM_IT_CCx(chHi), DISABLE);
// setup callback info // setup callback info
@ -377,11 +377,11 @@ void timerChConfigCallbacksDual(const timerHardware_t *timHw, timerCCHandlerRec_
timerConfig[timerIndex].overflowCallback[channelIndex + 1] = NULL; timerConfig[timerIndex].overflowCallback[channelIndex + 1] = NULL;
// enable channel IRQs // enable channel IRQs
if(edgeCallbackLo) { if (edgeCallbackLo) {
TIM_ClearFlag(timHw->tim, TIM_IT_CCx(chLo)); TIM_ClearFlag(timHw->tim, TIM_IT_CCx(chLo));
TIM_ITConfig(timHw->tim, TIM_IT_CCx(chLo), ENABLE); TIM_ITConfig(timHw->tim, TIM_IT_CCx(chLo), ENABLE);
} }
if(edgeCallbackHi) { if (edgeCallbackHi) {
TIM_ClearFlag(timHw->tim, TIM_IT_CCx(chHi)); TIM_ClearFlag(timHw->tim, TIM_IT_CCx(chHi));
TIM_ITConfig(timHw->tim, TIM_IT_CCx(chHi), ENABLE); TIM_ITConfig(timHw->tim, TIM_IT_CCx(chHi), ENABLE);
} }
@ -427,8 +427,8 @@ static unsigned getFilter(unsigned ticks)
16*5, 16*6, 16*8, 16*5, 16*6, 16*8,
32*5, 32*6, 32*8 32*5, 32*6, 32*8
}; };
for(unsigned i = 1; i < ARRAYLEN(ftab); i++) for (unsigned i = 1; i < ARRAYLEN(ftab); i++)
if(ftab[i] > ticks) if (ftab[i] > ticks)
return i - 1; return i - 1;
return 0x0f; return 0x0f;
} }
@ -503,7 +503,7 @@ void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHig
TIM_OCInitTypeDef TIM_OCInitStructure; TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCStructInit(&TIM_OCInitStructure);
if(outEnable) { if (outEnable) {
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Inactive; TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Inactive;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
if (timHw->output & TIMER_OUTPUT_INVERTED) { if (timHw->output & TIMER_OUTPUT_INVERTED) {
@ -540,17 +540,17 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
unsigned tim_status; unsigned tim_status;
tim_status = tim->SR & tim->DIER; tim_status = tim->SR & tim->DIER;
#if 1 #if 1
while(tim_status) { while (tim_status) {
// flags will be cleared by reading CCR in dual capture, make sure we call handler correctly // flags will be cleared by reading CCR in dual capture, make sure we call handler correctly
// currrent order is highest bit first. Code should not rely on specific order (it will introduce race conditions anyway) // currrent order is highest bit first. Code should not rely on specific order (it will introduce race conditions anyway)
unsigned bit = __builtin_clz(tim_status); unsigned bit = __builtin_clz(tim_status);
unsigned mask = ~(0x80000000 >> bit); unsigned mask = ~(0x80000000 >> bit);
tim->SR = mask; tim->SR = mask;
tim_status &= mask; tim_status &= mask;
switch(bit) { switch (bit) {
case __builtin_clz(TIM_IT_Update): { case __builtin_clz(TIM_IT_Update): {
if(timerConfig->forcedOverflowTimerValue != 0){ if (timerConfig->forcedOverflowTimerValue != 0){
capture = timerConfig->forcedOverflowTimerValue - 1; capture = timerConfig->forcedOverflowTimerValue - 1;
timerConfig->forcedOverflowTimerValue = 0; timerConfig->forcedOverflowTimerValue = 0;
} else { } else {
@ -558,7 +558,7 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
} }
timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive; timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
while(cb) { while (cb) {
cb->fn(cb, capture); cb->fn(cb, capture);
cb = cb->next; cb = cb->next;
} }
@ -583,7 +583,7 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
tim->SR = ~TIM_IT_Update; tim->SR = ~TIM_IT_Update;
capture = tim->ARR; capture = tim->ARR;
timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive; timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
while(cb) { while (cb) {
cb->fn(cb, capture); cb->fn(cb, capture);
cb = cb->next; cb = cb->next;
} }
@ -710,10 +710,10 @@ void timerInit(void)
#endif #endif
// initialize timer channel structures // initialize timer channel structures
for(int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
timerChannelInfo[i].type = TYPE_FREE; timerChannelInfo[i].type = TYPE_FREE;
} }
for(int i = 0; i < USED_TIMER_COUNT; i++) { for (int i = 0; i < USED_TIMER_COUNT; i++) {
timerInfo[i].priority = ~0; timerInfo[i].priority = ~0;
} }
} }
@ -724,18 +724,18 @@ void timerInit(void)
void timerStart(void) void timerStart(void)
{ {
#if 0 #if 0
for(unsigned timer = 0; timer < USED_TIMER_COUNT; timer++) { for (unsigned timer = 0; timer < USED_TIMER_COUNT; timer++) {
int priority = -1; int priority = -1;
int irq = -1; int irq = -1;
for(unsigned hwc = 0; hwc < USABLE_TIMER_CHANNEL_COUNT; hwc++) for (unsigned hwc = 0; hwc < USABLE_TIMER_CHANNEL_COUNT; hwc++)
if((timerChannelInfo[hwc].type != TYPE_FREE) && (timerHardware[hwc].tim == usedTimers[timer])) { if ((timerChannelInfo[hwc].type != TYPE_FREE) && (timerHardware[hwc].tim == usedTimers[timer])) {
// TODO - move IRQ to timer info // TODO - move IRQ to timer info
irq = timerHardware[hwc].irq; irq = timerHardware[hwc].irq;
} }
// TODO - aggregate required timer paramaters // TODO - aggregate required timer paramaters
configTimeBase(usedTimers[timer], 0, 1); configTimeBase(usedTimers[timer], 0, 1);
TIM_Cmd(usedTimers[timer], ENABLE); TIM_Cmd(usedTimers[timer], ENABLE);
if(priority >= 0) { // maybe none of the channels was configured if (priority >= 0) { // maybe none of the channels was configured
NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = irq; NVIC_InitStructure.NVIC_IRQChannel = irq;

View file

@ -86,7 +86,7 @@ static uint8_t lookupTimerIndex(const TIM_TypeDef *tim)
#define _CASE(i) _CASE_(TIM##i##_BASE, TIMER_INDEX(i)) #define _CASE(i) _CASE_(TIM##i##_BASE, TIMER_INDEX(i))
// let gcc do the work, switch should be quite optimized // let gcc do the work, switch should be quite optimized
switch((unsigned)tim >> _CASE_SHF) { switch ((unsigned)tim >> _CASE_SHF) {
#if USED_TIMERS & TIM_N(1) #if USED_TIMERS & TIM_N(1)
_CASE(1); _CASE(1);
#endif #endif
@ -247,7 +247,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
if (timerIndex >= USED_TIMER_COUNT) { if (timerIndex >= USED_TIMER_COUNT) {
return; return;
} }
if(timeHandle[timerIndex].Handle.Instance == tim) if (timeHandle[timerIndex].Handle.Instance == tim)
{ {
// already configured // already configured
return; return;
@ -256,7 +256,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
timeHandle[timerIndex].Handle.Instance = tim; timeHandle[timerIndex].Handle.Instance = tim;
timeHandle[timerIndex].Handle.Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR timeHandle[timerIndex].Handle.Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
if(tim == TIM1 || tim == TIM8 || tim == TIM9 || tim == TIM10 || tim == TIM11) if (tim == TIM1 || tim == TIM8 || tim == TIM9 || tim == TIM10 || tim == TIM11)
timeHandle[timerIndex].Handle.Init.Prescaler = (HAL_RCC_GetPCLK2Freq() * 2 / ((uint32_t)mhz * 1000000)) - 1; timeHandle[timerIndex].Handle.Init.Prescaler = (HAL_RCC_GetPCLK2Freq() * 2 / ((uint32_t)mhz * 1000000)) - 1;
else else
timeHandle[timerIndex].Handle.Init.Prescaler = (HAL_RCC_GetPCLK1Freq() * 2 * 2 / ((uint32_t)mhz * 1000000)) - 1; timeHandle[timerIndex].Handle.Init.Prescaler = (HAL_RCC_GetPCLK1Freq() * 2 * 2 / ((uint32_t)mhz * 1000000)) - 1;
@ -266,7 +266,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
timeHandle[timerIndex].Handle.Init.RepetitionCounter = 0x0000; timeHandle[timerIndex].Handle.Init.RepetitionCounter = 0x0000;
HAL_TIM_Base_Init(&timeHandle[timerIndex].Handle); HAL_TIM_Base_Init(&timeHandle[timerIndex].Handle);
if(tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 || tim == TIM9) if (tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 || tim == TIM9)
{ {
TIM_ClockConfigTypeDef sClockSourceConfig; TIM_ClockConfigTypeDef sClockSourceConfig;
sClockSourceConfig.ClockFilter = 0; sClockSourceConfig.ClockFilter = 0;
@ -278,7 +278,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
return; return;
} }
} }
if(tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 ) if (tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 )
{ {
TIM_MasterConfigTypeDef sMasterConfig; TIM_MasterConfigTypeDef sMasterConfig;
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
@ -306,7 +306,7 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui
uint8_t irq = timerInputIrq(timerHardwarePtr->tim); uint8_t irq = timerInputIrq(timerHardwarePtr->tim);
timerNVICConfigure(irq); timerNVICConfigure(irq);
// HACK - enable second IRQ on timers that need it // HACK - enable second IRQ on timers that need it
switch(irq) { switch (irq) {
case TIM1_CC_IRQn: case TIM1_CC_IRQn:
timerNVICConfigure(TIM1_UP_TIM10_IRQn); timerNVICConfigure(TIM1_UP_TIM10_IRQn);
@ -326,14 +326,14 @@ void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriori
return; return;
} }
unsigned channel = timHw - timerHardware; unsigned channel = timHw - timerHardware;
if(channel >= USABLE_TIMER_CHANNEL_COUNT) if (channel >= USABLE_TIMER_CHANNEL_COUNT)
return; return;
timerChannelInfo[channel].type = type; timerChannelInfo[channel].type = type;
unsigned timer = lookupTimerIndex(timHw->tim); unsigned timer = lookupTimerIndex(timHw->tim);
if(timer >= USED_TIMER_COUNT) if (timer >= USED_TIMER_COUNT)
return; return;
if(irqPriority < timerInfo[timer].priority) { if (irqPriority < timerInfo[timer].priority) {
// it would be better to set priority in the end, but current startup sequence is not ready // it would be better to set priority in the end, but current startup sequence is not ready
configTimeBase(usedTimers[timer], 0, 1); configTimeBase(usedTimers[timer], 0, 1);
HAL_TIM_Base_Start(&timeHandle[timerIndex].Handle); HAL_TIM_Base_Start(&timeHandle[timerIndex].Handle);
@ -365,15 +365,15 @@ static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, TIM_TypeDef *tim) {
timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive; timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive;
ATOMIC_BLOCK(NVIC_PRIO_TIMER) { ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
for(int i = 0; i < CC_CHANNELS_PER_TIMER; i++) for (int i = 0; i < CC_CHANNELS_PER_TIMER; i++)
if(cfg->overflowCallback[i]) { if (cfg->overflowCallback[i]) {
*chain = cfg->overflowCallback[i]; *chain = cfg->overflowCallback[i];
chain = &cfg->overflowCallback[i]->next; chain = &cfg->overflowCallback[i]->next;
} }
*chain = NULL; *chain = NULL;
} }
// enable or disable IRQ // enable or disable IRQ
if(cfg->overflowCallbackActive) if (cfg->overflowCallbackActive)
__HAL_TIM_ENABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_UPDATE); __HAL_TIM_ENABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_UPDATE);
else else
__HAL_TIM_DISABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_UPDATE); __HAL_TIM_DISABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_UPDATE);
@ -387,13 +387,13 @@ void timerChConfigCallbacks(const timerHardware_t *timHw, timerCCHandlerRec_t *e
return; return;
} }
uint8_t channelIndex = lookupChannelIndex(timHw->channel); uint8_t channelIndex = lookupChannelIndex(timHw->channel);
if(edgeCallback == NULL) // disable irq before changing callback to NULL if (edgeCallback == NULL) // disable irq before changing callback to NULL
__HAL_TIM_DISABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); __HAL_TIM_DISABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
// setup callback info // setup callback info
timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallback; timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallback;
timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback; timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback;
// enable channel IRQ // enable channel IRQ
if(edgeCallback) if (edgeCallback)
__HAL_TIM_ENABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); __HAL_TIM_ENABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim); timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim);
@ -412,9 +412,9 @@ void timerChConfigCallbacksDual(const timerHardware_t *timHw, timerCCHandlerRec_
uint16_t chHi = timHw->channel | TIM_CHANNEL_2; // upper channel uint16_t chHi = timHw->channel | TIM_CHANNEL_2; // upper channel
uint8_t channelIndex = lookupChannelIndex(chLo); // get index of lower channel uint8_t channelIndex = lookupChannelIndex(chLo); // get index of lower channel
if(edgeCallbackLo == NULL) // disable irq before changing setting callback to NULL if (edgeCallbackLo == NULL) // disable irq before changing setting callback to NULL
__HAL_TIM_DISABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(chLo)); __HAL_TIM_DISABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
if(edgeCallbackHi == NULL) // disable irq before changing setting callback to NULL if (edgeCallbackHi == NULL) // disable irq before changing setting callback to NULL
__HAL_TIM_DISABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(chHi)); __HAL_TIM_DISABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
// setup callback info // setup callback info
@ -424,11 +424,11 @@ void timerChConfigCallbacksDual(const timerHardware_t *timHw, timerCCHandlerRec_
timerConfig[timerIndex].overflowCallback[channelIndex + 1] = NULL; timerConfig[timerIndex].overflowCallback[channelIndex + 1] = NULL;
// enable channel IRQs // enable channel IRQs
if(edgeCallbackLo) { if (edgeCallbackLo) {
__HAL_TIM_CLEAR_FLAG(&timeHandle[timerIndex].Handle, TIM_IT_CCx(chLo)); __HAL_TIM_CLEAR_FLAG(&timeHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
__HAL_TIM_ENABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(chLo)); __HAL_TIM_ENABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
} }
if(edgeCallbackHi) { if (edgeCallbackHi) {
__HAL_TIM_CLEAR_FLAG(&timeHandle[timerIndex].Handle, TIM_IT_CCx(chHi)); __HAL_TIM_CLEAR_FLAG(&timeHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
__HAL_TIM_ENABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(chHi)); __HAL_TIM_ENABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
} }
@ -447,7 +447,7 @@ void timerChITConfigDualLo(const timerHardware_t *timHw, FunctionalState newStat
return; return;
} }
if(newState) if (newState)
__HAL_TIM_ENABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2)); __HAL_TIM_ENABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2));
else else
__HAL_TIM_DISABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2)); __HAL_TIM_DISABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2));
@ -466,7 +466,7 @@ void timerChITConfig(const timerHardware_t *timHw, FunctionalState newState)
return; return;
} }
if(newState) if (newState)
__HAL_TIM_ENABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); __HAL_TIM_ENABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
else else
__HAL_TIM_DISABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); __HAL_TIM_DISABLE_IT(&timeHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
@ -509,8 +509,8 @@ static unsigned getFilter(unsigned ticks)
16*5, 16*6, 16*8, 16*5, 16*6, 16*8,
32*5, 32*6, 32*8 32*5, 32*6, 32*8
}; };
for(unsigned i = 1; i < ARRAYLEN(ftab); i++) for (unsigned i = 1; i < ARRAYLEN(ftab); i++)
if(ftab[i] > ticks) if (ftab[i] > ticks)
return i - 1; return i - 1;
return 0x0f; return 0x0f;
} }
@ -519,7 +519,7 @@ static unsigned getFilter(unsigned ticks)
void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks) void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks)
{ {
unsigned timer = lookupTimerIndex(timHw->tim); unsigned timer = lookupTimerIndex(timHw->tim);
if(timer >= USED_TIMER_COUNT) if (timer >= USED_TIMER_COUNT)
return; return;
TIM_IC_InitTypeDef TIM_ICInitStructure; TIM_IC_InitTypeDef TIM_ICInitStructure;
@ -536,7 +536,7 @@ void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned
void timerChConfigICDual(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks) void timerChConfigICDual(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks)
{ {
unsigned timer = lookupTimerIndex(timHw->tim); unsigned timer = lookupTimerIndex(timHw->tim);
if(timer >= USED_TIMER_COUNT) if (timer >= USED_TIMER_COUNT)
return; return;
TIM_IC_InitTypeDef TIM_ICInitStructure; TIM_IC_InitTypeDef TIM_ICInitStructure;
@ -586,7 +586,7 @@ volatile timCCR_t* timerChCCR(const timerHardware_t *timHw)
void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHigh) void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHigh)
{ {
unsigned timer = lookupTimerIndex(timHw->tim); unsigned timer = lookupTimerIndex(timHw->tim);
if(timer >= USED_TIMER_COUNT) if (timer >= USED_TIMER_COUNT)
return; return;
TIM_OC_InitTypeDef TIM_OCInitStructure; TIM_OC_InitTypeDef TIM_OCInitStructure;
@ -600,7 +600,7 @@ void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHig
HAL_TIM_OC_ConfigChannel(&timeHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel); HAL_TIM_OC_ConfigChannel(&timeHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel);
if(outEnable) { if (outEnable) {
TIM_OCInitStructure.OCMode = TIM_OCMODE_INACTIVE; TIM_OCInitStructure.OCMode = TIM_OCMODE_INACTIVE;
HAL_TIM_OC_ConfigChannel(&timeHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel); HAL_TIM_OC_ConfigChannel(&timeHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel);
HAL_TIM_OC_Start(&timeHandle[timer].Handle, timHw->channel); HAL_TIM_OC_Start(&timeHandle[timer].Handle, timHw->channel);
@ -617,17 +617,17 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
unsigned tim_status; unsigned tim_status;
tim_status = tim->SR & tim->DIER; tim_status = tim->SR & tim->DIER;
#if 1 #if 1
while(tim_status) { while (tim_status) {
// flags will be cleared by reading CCR in dual capture, make sure we call handler correctly // flags will be cleared by reading CCR in dual capture, make sure we call handler correctly
// currrent order is highest bit first. Code should not rely on specific order (it will introduce race conditions anyway) // currrent order is highest bit first. Code should not rely on specific order (it will introduce race conditions anyway)
unsigned bit = __builtin_clz(tim_status); unsigned bit = __builtin_clz(tim_status);
unsigned mask = ~(0x80000000 >> bit); unsigned mask = ~(0x80000000 >> bit);
tim->SR = mask; tim->SR = mask;
tim_status &= mask; tim_status &= mask;
switch(bit) { switch (bit) {
case __builtin_clz(TIM_IT_UPDATE): { case __builtin_clz(TIM_IT_UPDATE): {
if(timerConfig->forcedOverflowTimerValue != 0){ if (timerConfig->forcedOverflowTimerValue != 0){
capture = timerConfig->forcedOverflowTimerValue - 1; capture = timerConfig->forcedOverflowTimerValue - 1;
timerConfig->forcedOverflowTimerValue = 0; timerConfig->forcedOverflowTimerValue = 0;
} else { } else {
@ -635,7 +635,7 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
} }
timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive; timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
while(cb) { while (cb) {
cb->fn(cb, capture); cb->fn(cb, capture);
cb = cb->next; cb = cb->next;
} }
@ -660,7 +660,7 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
tim->SR = ~TIM_IT_Update; tim->SR = ~TIM_IT_Update;
capture = tim->ARR; capture = tim->ARR;
timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive; timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
while(cb) { while (cb) {
cb->fn(cb, capture); cb->fn(cb, capture);
cb = cb->next; cb = cb->next;
} }
@ -815,10 +815,10 @@ void timerInit(void)
#endif #endif
// initialize timer channel structures // initialize timer channel structures
for(int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
timerChannelInfo[i].type = TYPE_FREE; timerChannelInfo[i].type = TYPE_FREE;
} }
for(int i = 0; i < USED_TIMER_COUNT; i++) { for (int i = 0; i < USED_TIMER_COUNT; i++) {
timerInfo[i].priority = ~0; timerInfo[i].priority = ~0;
} }
} }
@ -829,18 +829,18 @@ void timerInit(void)
void timerStart(void) void timerStart(void)
{ {
#if 0 #if 0
for(unsigned timer = 0; timer < USED_TIMER_COUNT; timer++) { for (unsigned timer = 0; timer < USED_TIMER_COUNT; timer++) {
int priority = -1; int priority = -1;
int irq = -1; int irq = -1;
for(unsigned hwc = 0; hwc < USABLE_TIMER_CHANNEL_COUNT; hwc++) for (unsigned hwc = 0; hwc < USABLE_TIMER_CHANNEL_COUNT; hwc++)
if((timerChannelInfo[hwc].type != TYPE_FREE) && (timerHardware[hwc].tim == usedTimers[timer])) { if ((timerChannelInfo[hwc].type != TYPE_FREE) && (timerHardware[hwc].tim == usedTimers[timer])) {
// TODO - move IRQ to timer info // TODO - move IRQ to timer info
irq = timerHardware[hwc].irq; irq = timerHardware[hwc].irq;
} }
// TODO - aggregate required timer paramaters // TODO - aggregate required timer paramaters
configTimeBase(usedTimers[timer], 0, 1); configTimeBase(usedTimers[timer], 0, 1);
TIM_Cmd(usedTimers[timer], ENABLE); TIM_Cmd(usedTimers[timer], ENABLE);
if(priority >= 0) { // maybe none of the channels was configured if (priority >= 0) { // maybe none of the channels was configured
NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = irq; NVIC_InitStructure.NVIC_IRQChannel = irq;

View file

@ -51,7 +51,7 @@ void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t
tmp = (uint32_t) TIMx; tmp = (uint32_t) TIMx;
tmp += CCMR_Offset; tmp += CCMR_Offset;
if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) { if ((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) {
tmp += (TIM_Channel>>1); tmp += (TIM_Channel>>1);
/* Reset the OCxM bits in the CCMRx register */ /* Reset the OCxM bits in the CCMRx register */

View file

@ -51,7 +51,7 @@ void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t
tmp = (uint32_t) TIMx; tmp = (uint32_t) TIMx;
tmp += CCMR_Offset; tmp += CCMR_Offset;
if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) { if ((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) {
tmp += (TIM_Channel>>1); tmp += (TIM_Channel>>1);
/* Reset the OCxM bits in the CCMRx register */ /* Reset the OCxM bits in the CCMRx register */

View file

@ -47,7 +47,7 @@ void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t
tmp = (uint32_t) TIMx; tmp = (uint32_t) TIMx;
tmp += CCMR_Offset; tmp += CCMR_Offset;
if((TIM_Channel == TIM_CHANNEL_1) ||(TIM_Channel == TIM_CHANNEL_3)) { if ((TIM_Channel == TIM_CHANNEL_1) ||(TIM_Channel == TIM_CHANNEL_3)) {
tmp += (TIM_Channel>>1); tmp += (TIM_Channel>>1);
/* Reset the OCxM bits in the CCMRx register */ /* Reset the OCxM bits in the CCMRx register */

View file

@ -1096,7 +1096,7 @@ static void printValuePointer(const clivalue_t *var, const void *valuePointer, u
return; // return from case for float only return; // return from case for float only
} }
switch(var->type & VALUE_MODE_MASK) { switch (var->type & VALUE_MODE_MASK) {
case MODE_MAX: case MODE_MAX:
case MODE_DIRECT: case MODE_DIRECT:
if ((var->type & VALUE_TYPE_MASK) == VAR_UINT32) if ((var->type & VALUE_TYPE_MASK) == VAR_UINT32)
@ -1529,7 +1529,7 @@ static void cliSerial(char *cmdline)
break; break;
} }
switch(i) { switch (i) {
case 0: case 0:
if (baudRateIndex < BAUD_1200 || baudRateIndex > BAUD_2470000) { if (baudRateIndex < BAUD_1200 || baudRateIndex > BAUD_2470000) {
continue; continue;
@ -1582,7 +1582,7 @@ static void cliSerialPassthrough(char *cmdline)
int index = 0; int index = 0;
while (tok != NULL) { while (tok != NULL) {
switch(index) { switch (index) {
case 0: case 0:
id = atoi(tok); id = atoi(tok);
break; break;
@ -2038,7 +2038,7 @@ static void cliModeColor(char *cmdline)
int modeIdx = args[MODE]; int modeIdx = args[MODE];
int funIdx = args[FUNCTION]; int funIdx = args[FUNCTION];
int color = args[COLOR]; int color = args[COLOR];
if(!setModeColor(modeIdx, funIdx, color)) { if (!setModeColor(modeIdx, funIdx, color)) {
cliShowParseError(); cliShowParseError();
return; return;
} }
@ -2697,7 +2697,7 @@ static void cliMap(char *cmdline)
static const char *checkCommand(const char *cmdLine, const char *command) static const char *checkCommand(const char *cmdLine, const char *command)
{ {
if(!strncasecmp(cmdLine, command, strlen(command)) // command names match if (!strncasecmp(cmdLine, command, strlen(command)) // command names match
&& !isalnum((unsigned)cmdLine[strlen(command)])) { // next characted in bufffer is not alphanumeric (command is correctly terminated) && !isalnum((unsigned)cmdLine[strlen(command)])) { // next characted in bufffer is not alphanumeric (command is correctly terminated)
return cmdLine + strlen(command) + 1; return cmdLine + strlen(command) + 1;
} else { } else {
@ -3016,7 +3016,7 @@ static void cliSet(char *cmdline)
switch (mode) { switch (mode) {
case MODE_MAX: case MODE_MAX:
case MODE_DIRECT: { case MODE_DIRECT: {
if(*eqptr != 0 && strspn(eqptr, "0123456789.+-") == strlen(eqptr)) { if (*eqptr != 0 && strspn(eqptr, "0123456789.+-") == strlen(eqptr)) {
int32_t value = 0; int32_t value = 0;
uint32_t uvalue = 0; uint32_t uvalue = 0;
float valuef = 0; float valuef = 0;
@ -3613,11 +3613,11 @@ void cliProcess(void)
const clicmd_t *cmd; const clicmd_t *cmd;
for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) { for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) {
if(!strncasecmp(cliBuffer, cmd->name, strlen(cmd->name)) // command names match if (!strncasecmp(cliBuffer, cmd->name, strlen(cmd->name)) // command names match
&& !isalnum((unsigned)cliBuffer[strlen(cmd->name)])) // next characted in bufffer is not alphanumeric (command is correctly terminated) && !isalnum((unsigned)cliBuffer[strlen(cmd->name)])) // next characted in bufffer is not alphanumeric (command is correctly terminated)
break; break;
} }
if(cmd < cmdTable + ARRAYLEN(cmdTable)) if (cmd < cmdTable + ARRAYLEN(cmdTable))
cmd->func(cliBuffer + strlen(cmd->name) + 1); cmd->func(cliBuffer + strlen(cmd->name) + 1);
else else
cliPrint("Unknown command, try 'help'"); cliPrint("Unknown command, try 'help'");

View file

@ -669,7 +669,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255)); sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255));
sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
sbufWriteU16(dst, rssi); sbufWriteU16(dst, rssi);
if(batteryConfig()->multiwiiCurrentMeterOutput) { if (batteryConfig()->multiwiiCurrentMeterOutput) {
sbufWriteU16(dst, (uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero sbufWriteU16(dst, (uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
} else } else
sbufWriteU16(dst, (int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A sbufWriteU16(dst, (int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A

View file

@ -206,7 +206,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
beeperConfirmationBeeps(1); beeperConfirmationBeeps(1);
} }
int newValue; int newValue;
switch(adjustmentFunction) { switch (adjustmentFunction) {
case ADJUSTMENT_RC_EXPO: case ADJUSTMENT_RC_EXPO:
newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->rcExpo8 = newValue; controlRateConfig->rcExpo8 = newValue;
@ -316,7 +316,7 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
{ {
bool applied = false; bool applied = false;
switch(adjustmentFunction) { switch (adjustmentFunction) {
case ADJUSTMENT_RATE_PROFILE: case ADJUSTMENT_RATE_PROFILE:
if (getCurrentControlRateProfile() != position) { if (getCurrentControlRateProfile() != position) {
changeControlRateProfile(position); changeControlRateProfile(position);

View file

@ -13,7 +13,7 @@ void statsOnDisarm(void);
#else #else
#define statsOnArm() do {} while(0) #define statsOnArm() do {} while (0)
#define statsOnDisarm() do {} while(0) #define statsOnDisarm() do {} while (0)
#endif #endif

View file

@ -343,7 +343,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
} }
// Compute and apply integral feedback if enabled // Compute and apply integral feedback if enabled
if(imuRuntimeConfig.dcm_ki_mag > 0.0f) { if (imuRuntimeConfig.dcm_ki_mag > 0.0f) {
// Stop integrating if spinning beyond the certain limit // Stop integrating if spinning beyond the certain limit
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) { if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
integralMagX += imuRuntimeConfig.dcm_ki_mag * ex * dt; // integral error scaled by Ki integralMagX += imuRuntimeConfig.dcm_ki_mag * ex * dt; // integral error scaled by Ki
@ -382,7 +382,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
ez = (ax * rMat[2][1] - ay * rMat[2][0]); ez = (ax * rMat[2][1] - ay * rMat[2][0]);
// Compute and apply integral feedback if enabled // Compute and apply integral feedback if enabled
if(imuRuntimeConfig.dcm_ki_acc > 0.0f) { if (imuRuntimeConfig.dcm_ki_acc > 0.0f) {
// Stop integrating if spinning beyond the certain limit // Stop integrating if spinning beyond the certain limit
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) { if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
integralAccX += imuRuntimeConfig.dcm_ki_acc * ex * dt; // integral error scaled by Ki integralAccX += imuRuntimeConfig.dcm_ki_acc * ex * dt; // integral error scaled by Ki

View file

@ -213,7 +213,7 @@ bool pidInitFilters(void)
{ {
const uint32_t refreshRate = getPidUpdateRate(); const uint32_t refreshRate = getPidUpdateRate();
notchFilterApplyFn = nullFilterApply; notchFilterApplyFn = nullFilterApply;
if(refreshRate != 0 && pidProfile()->dterm_soft_notch_hz != 0){ if (refreshRate != 0 && pidProfile()->dterm_soft_notch_hz != 0){
notchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; notchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; ++ axis) { for (int axis = 0; axis < 3; ++ axis) {
biquadFilterInitNotch(&pidState[axis].deltaNotchFilter, refreshRate, pidProfile()->dterm_soft_notch_hz, pidProfile()->dterm_soft_notch_cutoff); biquadFilterInitNotch(&pidState[axis].deltaNotchFilter, refreshRate, pidProfile()->dterm_soft_notch_hz, pidProfile()->dterm_soft_notch_cutoff);

View file

@ -195,7 +195,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
const timeDelta_t stateTimeMs = currentTimeMs - tuneCurrent[axis].stateEnterTime; const timeDelta_t stateTimeMs = currentTimeMs - tuneCurrent[axis].stateEnterTime;
bool gainsUpdated = false; bool gainsUpdated = false;
switch(tuneCurrent[axis].state) { switch (tuneCurrent[axis].state) {
case DEMAND_TOO_LOW: case DEMAND_TOO_LOW:
break; break;
case DEMAND_OVERSHOOT: case DEMAND_OVERSHOOT:

View file

@ -258,7 +258,7 @@ void beeperConfirmationBeeps(uint8_t beepCount)
i = 0; i = 0;
cLimit = beepCount * 2; cLimit = beepCount * 2;
if(cLimit > MAX_MULTI_BEEPS) if (cLimit > MAX_MULTI_BEEPS)
cLimit = MAX_MULTI_BEEPS; //stay within array size cLimit = MAX_MULTI_BEEPS; //stay within array size
do { do {
beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_DURATION; // 20ms beep beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_DURATION; // 20ms beep

View file

@ -323,10 +323,10 @@ static void bitmapDecompressAndShow(uint8_t *bitmap)
bitmap++; bitmap++;
uint16_t bitmapSize = (width * height) / 8; uint16_t bitmapSize = (width * height) / 8;
for (i = 0; i < bitmapSize; i++) { for (i = 0; i < bitmapSize; i++) {
if(count == 0) { if (count == 0) {
data = *bitmap; data = *bitmap;
bitmap++; bitmap++;
if(data == *bitmap) { if (data == *bitmap) {
bitmap++; bitmap++;
count = *bitmap; count = *bitmap;
bitmap++; bitmap++;
@ -503,7 +503,7 @@ void dashboardUpdate(timeUs_t currentTimeUs)
return; return;
} }
switch(currentPageId) { switch (currentPageId) {
case PAGE_WELCOME: case PAGE_WELCOME:
showWelcomePage(); showWelcomePage();
break; break;

View file

@ -424,14 +424,14 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
waitForSerialPortToFinishTransmitting(gpsState.gpsPort); waitForSerialPortToFinishTransmitting(gpsState.gpsPort);
waitForSerialPortToFinishTransmitting(gpsPassthroughPort); waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
if(!(gpsState.gpsPort->mode & MODE_TX)) if (!(gpsState.gpsPort->mode & MODE_TX))
serialSetMode(gpsState.gpsPort, gpsState.gpsPort->mode | MODE_TX); serialSetMode(gpsState.gpsPort, gpsState.gpsPort->mode | MODE_TX);
LED0_OFF; LED0_OFF;
LED1_OFF; LED1_OFF;
char c; char c;
while(1) { while (1) {
if (serialRxBytesWaiting(gpsState.gpsPort)) { if (serialRxBytesWaiting(gpsState.gpsPort)) {
LED0_ON; LED0_ON;
c = serialRead(gpsState.gpsPort); c = serialRead(gpsState.gpsPort);

View file

@ -110,7 +110,7 @@ bool gpsPollI2CNAV(void)
bool gpsHandleI2CNAV(void) bool gpsHandleI2CNAV(void)
{ {
// Process state // Process state
switch(gpsState.state) { switch (gpsState.state) {
default: default:
return false; return false;

View file

@ -130,7 +130,7 @@ int32_t decodeLong(uint32_t idx, uint8_t mask)
{ {
union { uint32_t l; uint8_t b[4]; } val; union { uint32_t l; uint8_t b[4]; } val;
val.l=idx; val.l=idx;
for(int i = 0; i < 4; i++) val.b[i] ^= mask; for (int i = 0; i < 4; i++) val.b[i] ^= mask;
return val.l; return val.l;
} }
@ -138,7 +138,7 @@ int16_t decodeShort(uint16_t idx, uint8_t mask)
{ {
union { uint16_t s; uint8_t b[2]; } val; union { uint16_t s; uint8_t b[2]; } val;
val.s=idx; val.s=idx;
for(int i = 0; i < 2; i++) val.b[i] ^= mask; for (int i = 0; i < 2; i++) val.b[i] ^= mask;
return val.s; return val.s;
} }
@ -345,7 +345,7 @@ bool gpsHandleNAZA(void)
bool hasNewData = gpsReceiveData(); bool hasNewData = gpsReceiveData();
// Process state // Process state
switch(gpsState.state) { switch (gpsState.state) {
default: default:
return false; return false;

View file

@ -126,7 +126,7 @@ static bool gpsNewFrameNMEA(char c)
switch (gps_frame) { switch (gps_frame) {
case FRAME_GGA: //************* GPGGA FRAME parsing case FRAME_GGA: //************* GPGGA FRAME parsing
switch(param) { switch (param) {
// case 1: // Time information // case 1: // Time information
// break; // break;
case 2: case 2:
@ -162,7 +162,7 @@ static bool gpsNewFrameNMEA(char c)
} }
break; break;
case FRAME_RMC: //************* GPRMC FRAME parsing case FRAME_RMC: //************* GPRMC FRAME parsing
switch(param) { switch (param) {
case 7: case 7:
gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis
break; break;
@ -272,7 +272,7 @@ bool gpsHandleNMEA(void)
bool hasNewData = gpsReceiveData(); bool hasNewData = gpsReceiveData();
// Process state // Process state
switch(gpsState.state) { switch (gpsState.state) {
default: default:
return false; return false;

View file

@ -480,7 +480,7 @@ static bool gpsParceFrameUBLOX(void)
_new_speed = true; _new_speed = true;
break; break;
case MSG_VER: case MSG_VER:
if(_class == CLASS_MON) { if (_class == CLASS_MON) {
//uint32_t swver = _buffer.ver.swVersion; //uint32_t swver = _buffer.ver.swVersion;
gpsState.hwVersion = atoi(_buffer.ver.hwVersion); gpsState.hwVersion = atoi(_buffer.ver.hwVersion);
} }
@ -636,7 +636,7 @@ static bool gpsConfigure(void)
#ifdef GPS_PROTO_UBLOX_NEO7PLUS #ifdef GPS_PROTO_UBLOX_NEO7PLUS
configureMSG(MSG_CLASS_UBX, MSG_PVT, 0); configureMSG(MSG_CLASS_UBX, MSG_PVT, 0);
} }
else if(gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) { else if (gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) {
configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0); configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0); configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0);
configureMSG(MSG_CLASS_UBX, MSG_SOL, 0); configureMSG(MSG_CLASS_UBX, MSG_SOL, 0);
@ -655,7 +655,7 @@ static bool gpsConfigure(void)
configureRATE(200); // 5Hz configureRATE(200); // 5Hz
#ifdef GPS_PROTO_UBLOX_NEO7PLUS #ifdef GPS_PROTO_UBLOX_NEO7PLUS
} }
else if(gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) { else if (gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) {
configureRATE(100); // 10Hz configureRATE(100); // 10Hz
} }
#endif #endif
@ -751,7 +751,7 @@ bool gpsHandleUBLOX(void)
bool hasNewData = gpsReceiveData(); bool hasNewData = gpsReceiveData();
// Process state // Process state
switch(gpsState.state) { switch (gpsState.state) {
default: default:
return false; return false;

View file

@ -864,7 +864,7 @@ static void applyLedAnimationLayer(bool updateNow, timeUs_t *timer)
{ {
static uint8_t frameCounter = 0; static uint8_t frameCounter = 0;
const int animationFrames = ledGridHeight; const int animationFrames = ledGridHeight;
if(updateNow) { if (updateNow) {
frameCounter = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; frameCounter = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0;
*timer += LED_STRIP_HZ(20); *timer += LED_STRIP_HZ(20);
} }
@ -993,7 +993,7 @@ bool parseColor(int index, const char *colorConfig)
}; };
for (int componentIndex = 0; result && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) { for (int componentIndex = 0; result && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) {
int val = atoi(remainingCharacters); int val = atoi(remainingCharacters);
if(val > hsv_limit[componentIndex]) { if (val > hsv_limit[componentIndex]) {
result = false; result = false;
break; break;
} }
@ -1034,7 +1034,7 @@ bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex)
if (colorIndex < 0 || colorIndex >= LED_CONFIGURABLE_COLOR_COUNT) if (colorIndex < 0 || colorIndex >= LED_CONFIGURABLE_COLOR_COUNT)
return false; return false;
if (modeIndex < LED_MODE_COUNT) { // modeIndex_e is unsigned, so one-sided test is enough if (modeIndex < LED_MODE_COUNT) { // modeIndex_e is unsigned, so one-sided test is enough
if(modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT) if (modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT)
return false; return false;
ledStripConfigMutable()->modeColors[modeIndex].color[modeColorIndex] = colorIndex; ledStripConfigMutable()->modeColors[modeIndex].color[modeColorIndex] = colorIndex;
} else if (modeIndex == LED_SPECIAL) { } else if (modeIndex == LED_SPECIAL) {

View file

@ -319,7 +319,7 @@ serialPort_t *openSerialPort(
serialPort_t *serialPort = NULL; serialPort_t *serialPort = NULL;
switch(identifier) { switch (identifier) {
#ifdef USE_VCP #ifdef USE_VCP
case SERIAL_PORT_USB_VCP: case SERIAL_PORT_USB_VCP:
serialPort = usbVcpOpen(); serialPort = usbVcpOpen();
@ -526,7 +526,7 @@ void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer
// Either port might be open in a mode other than MODE_RXTX. We rely on // Either port might be open in a mode other than MODE_RXTX. We rely on
// serialRxBytesWaiting() to do the right thing for a TX only port. No // serialRxBytesWaiting() to do the right thing for a TX only port. No
// special handling is necessary OR performed. // special handling is necessary OR performed.
while(1) { while (1) {
// TODO: maintain a timestamp of last data received. Use this to // TODO: maintain a timestamp of last data received. Use this to
// implement a guard interval and check for `+++` as an escape sequence // implement a guard interval and check for `+++` as an escape sequence
// to return to CLI command mode. // to return to CLI command mode.

View file

@ -139,7 +139,7 @@ uint8_t esc4wayInit(void)
pwmIOConfiguration_t *pwmIOConfiguration = pwmGetOutputConfiguration(); pwmIOConfiguration_t *pwmIOConfiguration = pwmGetOutputConfiguration();
for (volatile uint8_t i = 0; i < pwmIOConfiguration->ioCount; i++) { for (volatile uint8_t i = 0; i < pwmIOConfiguration->ioCount; i++) {
if ((pwmIOConfiguration->ioConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { if ((pwmIOConfiguration->ioConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) {
if(motor[pwmIOConfiguration->ioConfigurations[i].index] > 0) { if (motor[pwmIOConfiguration->ioConfigurations[i].index] > 0) {
escHardware[escCount].io = IOGetByTag(pwmIOConfiguration->ioConfigurations[i].timerHardware->tag); escHardware[escCount].io = IOGetByTag(pwmIOConfiguration->ioConfigurations[i].timerHardware->tag);
setEscInput(escCount); setEscInput(escCount);
setEscHi(escCount); setEscHi(escCount);
@ -418,7 +418,7 @@ void esc4wayProcess(serialPort_t *mspPort)
#endif #endif
bool isExitScheduled = false; bool isExitScheduled = false;
while(1) { while (1) {
// restart looking for new sequence from host // restart looking for new sequence from host
do { do {
CRC_in.word = 0; CRC_in.word = 0;
@ -446,7 +446,7 @@ void esc4wayProcess(serialPort_t *mspPort)
CRC_check.bytes[1] = ReadByte(); CRC_check.bytes[1] = ReadByte();
CRC_check.bytes[0] = ReadByte(); CRC_check.bytes[0] = ReadByte();
if(CRC_check.word == CRC_in.word) { if (CRC_check.word == CRC_in.word) {
ACK_OUT = ACK_OK; ACK_OUT = ACK_OK;
} else { } else {
ACK_OUT = ACK_I_INVALID_CRC; ACK_OUT = ACK_I_INVALID_CRC;
@ -461,12 +461,12 @@ void esc4wayProcess(serialPort_t *mspPort)
ioMem.D_PTR_I = ParamBuf; ioMem.D_PTR_I = ParamBuf;
switch(CMD) { switch (CMD) {
// ******* Interface related stuff ******* // ******* Interface related stuff *******
case cmd_InterfaceTestAlive: case cmd_InterfaceTestAlive:
{ {
if (isMcuConnected()){ if (isMcuConnected()){
switch(CurrentInterfaceMode) switch (CurrentInterfaceMode)
{ {
#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
case imATM_BLB: case imATM_BLB:
@ -583,7 +583,7 @@ void esc4wayProcess(serialPort_t *mspPort)
} }
O_PARAM_LEN = DeviceInfoSize; //4 O_PARAM_LEN = DeviceInfoSize; //4
O_PARAM = (uint8_t *)&DeviceInfo; O_PARAM = (uint8_t *)&DeviceInfo;
if(Connect(&DeviceInfo)) { if (Connect(&DeviceInfo)) {
DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode; DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode;
} else { } else {
SET_DISCONNECTED; SET_DISCONNECTED;
@ -595,7 +595,7 @@ void esc4wayProcess(serialPort_t *mspPort)
#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
case cmd_DeviceEraseAll: case cmd_DeviceEraseAll:
{ {
switch(CurrentInterfaceMode) switch (CurrentInterfaceMode)
{ {
case imSK: case imSK:
{ {
@ -639,13 +639,13 @@ void esc4wayProcess(serialPort_t *mspPort)
wtf.D_FLASH_ADDR_L=Adress_L; wtf.D_FLASH_ADDR_L=Adress_L;
wtf.D_PTR_I = BUF_I; wtf.D_PTR_I = BUF_I;
*/ */
switch(CurrentInterfaceMode) switch (CurrentInterfaceMode)
{ {
#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
case imSIL_BLB: case imSIL_BLB:
case imATM_BLB: case imATM_BLB:
{ {
if(!BL_ReadFlash(CurrentInterfaceMode, &ioMem)) if (!BL_ReadFlash(CurrentInterfaceMode, &ioMem))
{ {
ACK_OUT = ACK_D_GENERAL_ERROR; ACK_OUT = ACK_D_GENERAL_ERROR;
} }
@ -655,7 +655,7 @@ void esc4wayProcess(serialPort_t *mspPort)
#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
case imSK: case imSK:
{ {
if(!Stk_ReadFlash(&ioMem)) if (!Stk_ReadFlash(&ioMem))
{ {
ACK_OUT = ACK_D_GENERAL_ERROR; ACK_OUT = ACK_D_GENERAL_ERROR;
} }
@ -706,7 +706,7 @@ void esc4wayProcess(serialPort_t *mspPort)
default: default:
ACK_OUT = ACK_I_INVALID_CMD; ACK_OUT = ACK_I_INVALID_CMD;
} }
if(ACK_OUT == ACK_OK) if (ACK_OUT == ACK_OK)
{ {
O_PARAM_LEN = ioMem.D_NUM_BYTES; O_PARAM_LEN = ioMem.D_NUM_BYTES;
O_PARAM = (uint8_t *)&ParamBuf; O_PARAM = (uint8_t *)&ParamBuf;

View file

@ -94,7 +94,7 @@ static uint8_t suart_getc_(uint8_t *bt)
uint16_t bitmask = 0; uint16_t bitmask = 0;
uint8_t bit = 0; uint8_t bit = 0;
while (micros() < btime); while (micros() < btime);
while(1) { while (1) {
if (ESC_IS_HI) if (ESC_IS_HI)
{ {
bitmask |= (1 << bit); bitmask |= (1 << bit);
@ -117,8 +117,8 @@ static void suart_putc_(uint8_t *tx_b)
// shift out stopbit first // shift out stopbit first
uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10); uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10);
uint32_t btime = micros(); uint32_t btime = micros();
while(1) { while (1) {
if(bitmask & 1) { if (bitmask & 1) {
ESC_SET_HI; // 1 ESC_SET_HI; // 1
} }
else { else {
@ -156,22 +156,22 @@ static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len)
LastCRC_16.word = 0; LastCRC_16.word = 0;
uint8_t LastACK = brNONE; uint8_t LastACK = brNONE;
do { do {
if(!suart_getc_(pstring)) goto timeout; if (!suart_getc_(pstring)) goto timeout;
ByteCrc(pstring); ByteCrc(pstring);
pstring++; pstring++;
len--; len--;
} while(len > 0); } while (len > 0);
if(isMcuConnected()) { if (isMcuConnected()) {
//With CRC read 3 more //With CRC read 3 more
if(!suart_getc_(&LastCRC_16.bytes[0])) goto timeout; if (!suart_getc_(&LastCRC_16.bytes[0])) goto timeout;
if(!suart_getc_(&LastCRC_16.bytes[1])) goto timeout; if (!suart_getc_(&LastCRC_16.bytes[1])) goto timeout;
if(!suart_getc_(&LastACK)) goto timeout; if (!suart_getc_(&LastACK)) goto timeout;
if (CRC_16.word != LastCRC_16.word) { if (CRC_16.word != LastCRC_16.word) {
LastACK = brERRORCRC; LastACK = brERRORCRC;
} }
} else { } else {
if(!suart_getc_(&LastACK)) goto timeout; if (!suart_getc_(&LastACK)) goto timeout;
} }
timeout: timeout:
return (LastACK == brSUCCESS); return (LastACK == brSUCCESS);
@ -260,7 +260,7 @@ void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo)
static uint8_t BL_SendCMDSetAddress(ioMem_t *pMem) //supports only 16 bit Adr static uint8_t BL_SendCMDSetAddress(ioMem_t *pMem) //supports only 16 bit Adr
{ {
// skip if adr == 0xFFFF // skip if adr == 0xFFFF
if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; if ((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1;
uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->D_FLASH_ADDR_H, pMem->D_FLASH_ADDR_L }; uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->D_FLASH_ADDR_H, pMem->D_FLASH_ADDR_L };
BL_SendBuf(sCMD, 4); BL_SendBuf(sCMD, 4);
return (BL_GetACK(2) == brSUCCESS); return (BL_GetACK(2) == brSUCCESS);
@ -302,7 +302,7 @@ static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout)
uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem) uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem)
{ {
if(interface_mode == imATM_BLB) { if (interface_mode == imATM_BLB) {
return BL_ReadA(CMD_READ_FLASH_ATM, pMem); return BL_ReadA(CMD_READ_FLASH_ATM, pMem);
} else { } else {
return BL_ReadA(CMD_READ_FLASH_SIL, pMem); return BL_ReadA(CMD_READ_FLASH_SIL, pMem);

View file

@ -177,9 +177,9 @@ static uint8_t StkReadLeader(void)
// Wait for the first bit // Wait for the first bit
uint32_t waitcycl; //250uS each uint32_t waitcycl; //250uS each
if((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) { if ((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) {
waitcycl = STK_WAITCYLCES_EXT; waitcycl = STK_WAITCYLCES_EXT;
} else if(StkCmd == CMD_SIGN_ON) { } else if (StkCmd == CMD_SIGN_ON) {
waitcycl = STK_WAITCYLCES_START; waitcycl = STK_WAITCYLCES_START;
} else { } else {
waitcycl= STK_WAITCYLCES; waitcycl= STK_WAITCYLCES;
@ -272,7 +272,7 @@ static uint8_t _CMD_LOAD_ADDRESS(ioMem_t *pMem)
{ {
// ignore 0xFFFF // ignore 0xFFFF
// assume address is set before and we read or write the immediately following package // assume address is set before and we read or write the immediately following package
if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; if ((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1;
StkCmd = CMD_LOAD_ADDRESS; StkCmd = CMD_LOAD_ADDRESS;
StkSendPacketHeader(); StkSendPacketHeader();
StkSendByte(0); // hi byte Msg len StkSendByte(0); // hi byte Msg len

View file

@ -168,17 +168,17 @@ static void frequencyConfigurator(uint32_t frequency)
{ {
uint8_t band; uint8_t band;
if(frequency<48000) { if (frequency<48000) {
frequency -= 24000; frequency -= 24000;
band = frequency/1000; band = frequency/1000;
if(band>23) band = 23; if (band>23) band = 23;
frequency -= (1000*(uint32_t)band); frequency -= (1000*(uint32_t)band);
frequency *= 64; frequency *= 64;
band |= 0x40; band |= 0x40;
} else { } else {
frequency -= 48000; frequency -= 48000;
band = frequency/2000; band = frequency/2000;
if(band>22) band = 22; if (band>22) band = 22;
frequency -= (2000*(uint32_t)band); frequency -= (2000*(uint32_t)band);
frequency *= 32; frequency *= 32;
band |= 0x60; band |= 0x60;
@ -199,7 +199,7 @@ static void rfm22bInitParameter(void)
static uint8_t cf3[8] = {0x0f,0x42,0x07,0x20,0x2d,0xd4,0x00,0x00}; static uint8_t cf3[8] = {0x0f,0x42,0x07,0x20,0x2d,0xd4,0x00,0x00};
rfmSpiRead(0x03); rfmSpiRead(0x03);
rfmSpiRead(0x04); rfmSpiRead(0x04);
for(i = 0; i < 8; i++) for (i = 0; i < 8; i++)
rfmSpiWrite(0x06+i, cf1[i]); rfmSpiWrite(0x06+i, cf1[i]);
if (first_init) { if (first_init) {
first_init = 0; first_init = 0;
@ -210,20 +210,20 @@ static void rfm22bInitParameter(void)
rfmSpiWrite(0x6f, 0xD5); rfmSpiWrite(0x6f, 0xD5);
rfmSpiWrite(0x1c, 0x02); rfmSpiWrite(0x1c, 0x02);
rfmSpiWrite(0x70, 0x00); rfmSpiWrite(0x70, 0x00);
for(i=0; i<6; i++) rfmSpiWrite(0x20+i, cf2[i]); for (i=0; i<6; i++) rfmSpiWrite(0x20+i, cf2[i]);
rfmSpiWrite(0x2a, 0x1e); rfmSpiWrite(0x2a, 0x1e);
rfmSpiWrite(0x72, 0x1F); rfmSpiWrite(0x72, 0x1F);
rfmSpiWrite(0x30, 0x8c); rfmSpiWrite(0x30, 0x8c);
rfmSpiWrite(0x3e, 22); rfmSpiWrite(0x3e, 22);
for(i=0; i<8; i++) rfmSpiWrite(0x32+i, cf3[i]); for (i=0; i<8; i++) rfmSpiWrite(0x32+i, cf3[i]);
for(i=0; i<4; i++) rfmSpiWrite(0x43+i, 0xff); for (i=0; i<4; i++) rfmSpiWrite(0x43+i, 0xff);
rfmSpiWrite(0x6d, eleresConfig()->eleresTelemetryPower | 0x18); rfmSpiWrite(0x6d, eleresConfig()->eleresTelemetryPower | 0x18);
rfmSpiWrite(0x79, 0x00); rfmSpiWrite(0x79, 0x00);
rfmSpiWrite(0x7a, 0x04); rfmSpiWrite(0x7a, 0x04);
rfmSpiWrite(0x71, 0x23); rfmSpiWrite(0x71, 0x23);
rfmSpiWrite(0x73, 0x00); rfmSpiWrite(0x73, 0x00);
rfmSpiWrite(0x74, 0x00); rfmSpiWrite(0x74, 0x00);
for(i=0; i<4; i++) { for (i=0; i<4; i++) {
rfmSpiWrite(0x3a+i, eleresSignaturePtr[i]); rfmSpiWrite(0x3a+i, eleresSignaturePtr[i]);
rfmSpiWrite(0x3f+i, eleresSignaturePtr[i]); rfmSpiWrite(0x3f+i, eleresSignaturePtr[i]);
} }
@ -236,15 +236,15 @@ static void rfm22bInitParameter(void)
static void channelHopping(uint8_t hops) static void channelHopping(uint8_t hops)
{ {
hoppingChannel += hops; hoppingChannel += hops;
while(hoppingChannel >= 16) hoppingChannel -= 16; while (hoppingChannel >= 16) hoppingChannel -= 16;
if (eleresConfig()->eleresTelemetryEn && eleresConfig()->eleresLocEn) { if (eleresConfig()->eleresTelemetryEn && eleresConfig()->eleresLocEn) {
if(bkgLocEnable && (hoppingChannel==bkgLocChlist || hoppingChannel==(bkgLocChlist+1)%16)) { if (bkgLocEnable && (hoppingChannel==bkgLocChlist || hoppingChannel==(bkgLocChlist+1)%16)) {
rfmSpiWrite(0x79,0); rfmSpiWrite(0x79,0);
bkgLocEnable = 2; bkgLocEnable = 2;
return; return;
} }
if(bkgLocEnable == 2) bkgLocEnable = 1; if (bkgLocEnable == 2) bkgLocEnable = 1;
} }
rfmSpiWrite(0x79, holList[hoppingChannel]); rfmSpiWrite(0x79, holList[hoppingChannel]);
@ -275,20 +275,20 @@ static void telemetryRX(void)
switch (telem_state++) { switch (telem_state++) {
case 0: case 0:
if(presfil>200000) pres = presfil/4 - 50000; if (presfil>200000) pres = presfil/4 - 50000;
else pres = 1; else pres = 1;
themp = (uint8_t)(thempfil/80 + 86); themp = (uint8_t)(thempfil/80 + 86);
if (FLIGHT_MODE(FAILSAFE_MODE)) wii_flymode = 7; if (FLIGHT_MODE(FAILSAFE_MODE)) wii_flymode = 7;
else if(FLIGHT_MODE(PASSTHRU_MODE)) wii_flymode = 8; else if (FLIGHT_MODE(PASSTHRU_MODE)) wii_flymode = 8;
else if(FLIGHT_MODE(NAV_RTH_MODE)) wii_flymode = 6; else if (FLIGHT_MODE(NAV_RTH_MODE)) wii_flymode = 6;
else if(FLIGHT_MODE(NAV_POSHOLD_MODE)) wii_flymode = 5; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) wii_flymode = 5;
else if(FLIGHT_MODE(HEADFREE_MODE)) wii_flymode = 4; else if (FLIGHT_MODE(HEADFREE_MODE)) wii_flymode = 4;
else if(FLIGHT_MODE(NAV_ALTHOLD_MODE)) wii_flymode = 3; else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) wii_flymode = 3;
else if(FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) wii_flymode = 2; else if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) wii_flymode = 2;
else wii_flymode = 1; else wii_flymode = 1;
if(ARMING_FLAG(ARMED)) wii_flymode |= 0x10; if (ARMING_FLAG(ARMED)) wii_flymode |= 0x10;
rfTxBuffer[0] = 0x54; rfTxBuffer[0] = 0x54;
rfTxBuffer[1] = localRssi; rfTxBuffer[1] = localRssi;
rfTxBuffer[2] = quality; rfTxBuffer[2] = quality;
@ -307,9 +307,9 @@ static void telemetryRX(void)
rfTxBuffer[0] = 0x50; rfTxBuffer[0] = 0x50;
cnv.val = gpsSol.llh.lat/10; cnv.val = gpsSol.llh.lat/10;
for(i=0; i<4; i++) rfTxBuffer[i+1] = cnv.b[i]; for (i=0; i<4; i++) rfTxBuffer[i+1] = cnv.b[i];
cnv.val = gpsSol.llh.lon/10; cnv.val = gpsSol.llh.lon/10;
for(i=0; i<4; i++) rfTxBuffer[i+5] = cnv.b[i]; for (i=0; i<4; i++) rfTxBuffer[i+5] = cnv.b[i];
rfTxBuffer[4] &= 0x0F; rfTxBuffer[4] &= 0x0F;
rfTxBuffer[4] |= (hdop << 4); rfTxBuffer[4] |= (hdop << 4);
@ -331,7 +331,7 @@ static void telemetryRX(void)
rfTxBuffer[0] = 0x47; rfTxBuffer[0] = 0x47;
rfTxBuffer[1] = (STATE(GPS_FIX)<<4) | (gpsSol.numSat & 0x0F); rfTxBuffer[1] = (STATE(GPS_FIX)<<4) | (gpsSol.numSat & 0x0F);
if(gpsSol.numSat > 15) rfTxBuffer[1] |= 0x80; if (gpsSol.numSat > 15) rfTxBuffer[1] |= 0x80;
rfTxBuffer[2] = ((course>>8) & 0x0F) | ((gpsspeed>>4) & 0xF0); rfTxBuffer[2] = ((course>>8) & 0x0F) | ((gpsspeed>>4) & 0xF0);
rfTxBuffer[3] = course & 0xFF; rfTxBuffer[3] = course & 0xFF;
rfTxBuffer[4] = gpsspeed & 0xFF; rfTxBuffer[4] = gpsspeed & 0xFF;
@ -382,15 +382,15 @@ static void parseStatusRegister(const uint8_t *payload)
static uint16_t rssifil; static uint16_t rssifil;
const timeMs_t irq_time = millis(); const timeMs_t irq_time = millis();
if((rfMode & RECEIVE) && (statusRegisters[0] & RF22B_RX_PACKET_RECEIVED_INTERRUPT)) if ((rfMode & RECEIVE) && (statusRegisters[0] & RF22B_RX_PACKET_RECEIVED_INTERRUPT))
rfMode |= RECEIVED; rfMode |= RECEIVED;
if((rfMode & TRANSMIT) && (statusRegisters[0] & RF22B_PACKET_SENT_INTERRUPT)) if ((rfMode & TRANSMIT) && (statusRegisters[0] & RF22B_PACKET_SENT_INTERRUPT))
rfMode |= TRANSMITTED; rfMode |= TRANSMITTED;
if((rfMode & RECEIVE) && (statusRegisters[1] & RF22B_VALID_SYNCWORD_INTERRUPT)) if ((rfMode & RECEIVE) && (statusRegisters[1] & RF22B_VALID_SYNCWORD_INTERRUPT))
rfMode |= PREAMBLE; rfMode |= PREAMBLE;
if(rfMode & RECEIVED) { if (rfMode & RECEIVED) {
if(bkgLocEnable < 2) { if (bkgLocEnable < 2) {
lastPackTime = irq_time; lastPackTime = irq_time;
nextPackTime = irq_time + channelHoppingTime; nextPackTime = irq_time + channelHoppingTime;
} }
@ -407,9 +407,9 @@ static void parseStatusRegister(const uint8_t *payload)
channelHoppingTime = (rfRxBuffer[20] & 0x0F)+18; channelHoppingTime = (rfRxBuffer[20] & 0x0F)+18;
dataReady |= DATA_FLAG; dataReady |= DATA_FLAG;
} else if (eleresConfig()->eleresLocEn && eleresConfig()->eleresTelemetryEn && bkgLocEnable==2) { } else if (eleresConfig()->eleresLocEn && eleresConfig()->eleresTelemetryEn && bkgLocEnable==2) {
if((rfRxBuffer[0] == 'H' && rfRxBuffer[2] == 'L') || if ((rfRxBuffer[0] == 'H' && rfRxBuffer[2] == 'L') ||
rfRxBuffer[0]=='T' || rfRxBuffer[0]=='P' || rfRxBuffer[0]=='G') { rfRxBuffer[0]=='T' || rfRxBuffer[0]=='P' || rfRxBuffer[0]=='G') {
if(bkgLocCnt==0) bkgLocCnt = 200; if (bkgLocCnt==0) bkgLocCnt = 200;
toReadyMode(); toReadyMode();
bkgLocEnable = 0; bkgLocEnable = 0;
channelHopping(0); channelHopping(0);
@ -424,7 +424,7 @@ static void parseStatusRegister(const uint8_t *payload)
} }
} }
if((dataReady & LOCALIZER_FLAG)==0) { if ((dataReady & LOCALIZER_FLAG)==0) {
if (eleresConfig()->eleresTelemetryEn) if (eleresConfig()->eleresTelemetryEn)
toTxMode(9); toTxMode(9);
else else
@ -432,27 +432,27 @@ static void parseStatusRegister(const uint8_t *payload)
} }
} }
if(rfMode & TRANSMITTED) { if (rfMode & TRANSMITTED) {
toReadyMode(); toReadyMode();
if(dataReady & LOCALIZER_FLAG) { if (dataReady & LOCALIZER_FLAG) {
rfmSpiWrite(0x79, holList[0]); rfmSpiWrite(0x79, holList[0]);
} else if(irq_time-lastPackTime <= 1500 && bkgLocEnable<2) } else if (irq_time-lastPackTime <= 1500 && bkgLocEnable<2)
channelHopping(1); channelHopping(1);
toRxMode(); toRxMode();
} }
if(rfMode & PREAMBLE) { if (rfMode & PREAMBLE) {
uint8_t rssitmp = rfmSpiRead(0x26); uint8_t rssitmp = rfmSpiRead(0x26);
if (eleresConfig()->eleresLocEn && eleresConfig()->eleresTelemetryEn && bkgLocEnable==2) { if (eleresConfig()->eleresLocEn && eleresConfig()->eleresTelemetryEn && bkgLocEnable==2) {
if(rssitmp>124) rssitmp = 124; if (rssitmp>124) rssitmp = 124;
if(rssitmp<18) rssitmp = 18; if (rssitmp<18) rssitmp = 18;
bkgLocBuf[0][1] = rssitmp + 128; bkgLocBuf[0][1] = rssitmp + 128;
} else { } else {
rssifil -= rssifil/8; rssifil -= rssifil/8;
rssifil += rssitmp; rssifil += rssitmp;
localRssi = (rssifil/8 * quality / 100)+10; localRssi = (rssifil/8 * quality / 100)+10;
if(localRssi>124) localRssi = 124; if (localRssi>124) localRssi = 124;
if(localRssi<18) localRssi = 18; if (localRssi<18) localRssi = 18;
} }
rfMode &= ~PREAMBLE; rfMode &= ~PREAMBLE;
} }
@ -480,8 +480,8 @@ void eleresSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
else else
led_time = cr_time; led_time = cr_time;
if((dataReady & LOCALIZER_FLAG) == 0) { if ((dataReady & LOCALIZER_FLAG) == 0) {
if(cr_time > nextPackTime+2) { if (cr_time > nextPackTime+2) {
if ((cr_time-lastPackTime > 1500) || firstRun) { if ((cr_time-lastPackTime > 1500) || firstRun) {
localRssi = 18; localRssi = 18;
rfm22bInitParameter(); rfm22bInitParameter();
@ -495,7 +495,7 @@ void eleresSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
// res = RX_FRAME_FAILSAFE; // res = RX_FRAME_FAILSAFE;
} else { } else {
if(cr_time-lastPackTime > 3*channelHoppingTime) { if (cr_time-lastPackTime > 3*channelHoppingTime) {
red_led_local=1; red_led_local=1;
if (localRssi > 0x18) if (localRssi > 0x18)
localRssi--; localRssi--;
@ -505,37 +505,37 @@ void eleresSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
nextPackTime += channelHoppingTime; nextPackTime += channelHoppingTime;
} }
} }
if(cr_time > qtest_time) { if (cr_time > qtest_time) {
qtest_time = cr_time + 500; qtest_time = cr_time + 500;
quality = goodFrames * 100 / (500/channelHoppingTime); quality = goodFrames * 100 / (500/channelHoppingTime);
if(quality > 100) quality = 100; if (quality > 100) quality = 100;
goodFrames = 0; goodFrames = 0;
} }
} }
if((dataReady & 0x03) == DATA_FLAG && rcData != NULL) { if ((dataReady & 0x03) == DATA_FLAG && rcData != NULL) {
if((dataReady & RELAY_FLAG)==0) { if ((dataReady & RELAY_FLAG)==0) {
channel_count = rfRxBuffer[20] >> 4; channel_count = rfRxBuffer[20] >> 4;
if(channel_count < 4) channel_count = 4; if (channel_count < 4) channel_count = 4;
if(channel_count > RC_CHANS) channel_count = 12; if (channel_count > RC_CHANS) channel_count = 12;
for(i = 0; i<channel_count; i++) { for (i = 0; i<channel_count; i++) {
temp_int = rfRxBuffer[i+1]; temp_int = rfRxBuffer[i+1];
if(i%2 == 0) if (i%2 == 0)
temp_int |= ((unsigned int)rfRxBuffer[i/2 + 13] << 4) & 0x0F00; temp_int |= ((unsigned int)rfRxBuffer[i/2 + 13] << 4) & 0x0F00;
else else
temp_int |= ((unsigned int)rfRxBuffer[i/2 + 13] << 8) & 0x0F00; temp_int |= ((unsigned int)rfRxBuffer[i/2 + 13] << 8) & 0x0F00;
if ((temp_int>799) && (temp_int<2201)) rcData4Values[i] = temp_int; if ((temp_int>799) && (temp_int<2201)) rcData4Values[i] = temp_int;
} }
n = rfRxBuffer[19]; n = rfRxBuffer[19];
for(i=channel_count; i < channel_count+5; i++) { for (i=channel_count; i < channel_count+5; i++) {
if(i > 11) break; if (i > 11) break;
if(n & 0x01) temp_int = BIN_ON_VALUE; if (n & 0x01) temp_int = BIN_ON_VALUE;
else temp_int = BIN_OFF_VALUE; else temp_int = BIN_OFF_VALUE;
rcData4Values[i] = temp_int; rcData4Values[i] = temp_int;
n >>= 1; n >>= 1;
} }
for(; i<RC_CHANS; i++) rcData4Values[i]=1500; for (; i<RC_CHANS; i++) rcData4Values[i]=1500;
for(i=0; i<RC_CHANS; i++) { for (i=0; i<RC_CHANS; i++) {
temp_int = rcData4Values[i]; temp_int = rcData4Values[i];
if (temp_int < rcData[i] -3) rcData[i] = temp_int+2; if (temp_int < rcData[i] -3) rcData[i] = temp_int+2;
if (temp_int > rcData[i] +3) rcData[i] = temp_int-2; if (temp_int > rcData[i] +3) rcData[i] = temp_int-2;
@ -545,10 +545,10 @@ void eleresSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
if (eleresConfig()->eleresTelemetryEn) { if (eleresConfig()->eleresTelemetryEn) {
if (eleresConfig()->eleresLocEn) { if (eleresConfig()->eleresLocEn) {
if(bkgLocEnable == 0) bkgLocCnt=0; if (bkgLocEnable == 0) bkgLocCnt=0;
if(bkgLocCnt) bkgLocCnt--; if (bkgLocCnt) bkgLocCnt--;
if(bkgLocCnt<128) if (bkgLocCnt<128)
telemetryRX(); telemetryRX();
else else
memcpy(rfTxBuffer, bkgLocBuf[bkgLocCnt%3], 9); memcpy(rfTxBuffer, bkgLocBuf[bkgLocCnt%3], 9);
@ -565,24 +565,24 @@ void eleresSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
} }
if (eleresConfig()->eleresLocEn) { if (eleresConfig()->eleresLocEn) {
if((dataReady & 0x03)==(DATA_FLAG | LOCALIZER_FLAG) && rfRxBuffer[19]<128) { if ((dataReady & 0x03)==(DATA_FLAG | LOCALIZER_FLAG) && rfRxBuffer[19]<128) {
if(rx_frames == 0) guard_time = lastPackTime; if (rx_frames == 0) guard_time = lastPackTime;
if(rx_frames < 250) { if (rx_frames < 250) {
rx_frames++; rx_frames++;
} }
if(rx_frames > 20 && cr_time-guard_time > (locForce?5000:20000)) { if (rx_frames > 20 && cr_time-guard_time > (locForce?5000:20000)) {
dataReady = 0; dataReady = 0;
localizerTime = cr_time + (1000L*eleresConfig()->eleresLocDelay); localizerTime = cr_time + (1000L*eleresConfig()->eleresLocDelay);
rfm22bInitParameter(); rfm22bInitParameter();
channelHopping(1); channelHopping(1);
rx_frames = 0; rx_frames = 0;
if(locForce && eleresConfig()->eleresTelemetryEn) { if (locForce && eleresConfig()->eleresTelemetryEn) {
bkgLocEnable = 1; bkgLocEnable = 1;
temp_int = 0; temp_int = 0;
for(i=0; i<16; i++) { for (i=0; i<16; i++) {
uint16_t mult = holList[i] * holList[(i+1)%16]; uint16_t mult = holList[i] * holList[(i+1)%16];
if(mult > temp_int) { if (mult > temp_int) {
temp_int = mult; temp_int = mult;
bkgLocChlist = i; bkgLocChlist = i;
} }
@ -591,11 +591,11 @@ void eleresSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
} }
} }
if(cr_time-lastPackTime > 8000) { if (cr_time-lastPackTime > 8000) {
rx_frames = 0; rx_frames = 0;
} }
if(!ARMING_FLAG(ARMED) && cr_time > localizerTime) { if (!ARMING_FLAG(ARMED) && cr_time > localizerTime) {
if((dataReady & LOCALIZER_FLAG)==0) { if ((dataReady & LOCALIZER_FLAG)==0) {
rfm22bInitParameter(); rfm22bInitParameter();
rfmSpiWrite(0x6d, eleresConfig()->eleresLocPower); rfmSpiWrite(0x6d, eleresConfig()->eleresLocPower);
} }
@ -607,7 +607,7 @@ void eleresSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
led_time = cr_time; led_time = cr_time;
bkgLocEnable = 0; bkgLocEnable = 0;
if(!(++loc_cnt & 1) && eleresConfig()->eleresTelemetryEn) { if (!(++loc_cnt & 1) && eleresConfig()->eleresTelemetryEn) {
telemetryRX(); telemetryRX();
toTxMode(9); toTxMode(9);
} else { } else {
@ -620,18 +620,18 @@ void eleresSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
} }
} }
if((ARMING_FLAG(ARMED) || firstRun) && (dataReady & LOCALIZER_FLAG)==0) if ((ARMING_FLAG(ARMED) || firstRun) && (dataReady & LOCALIZER_FLAG)==0)
localizerTime = cr_time + (1000L*eleresConfig()->eleresLocDelay); localizerTime = cr_time + (1000L*eleresConfig()->eleresLocDelay);
if (eleresConfig()->eleresTelemetryEn) if (eleresConfig()->eleresTelemetryEn)
if(dataReady & RELAY_FLAG) { if (dataReady & RELAY_FLAG) {
if(rfRxBuffer[0]=='H') bkgLocBuf[0][0]='T'; if (rfRxBuffer[0]=='H') bkgLocBuf[0][0]='T';
if(rfRxBuffer[0]=='T') { if (rfRxBuffer[0]=='T') {
bkgLocBuf[0][0]='T'; bkgLocBuf[0][0]='T';
memcpy(bkgLocBuf[0]+2, rfRxBuffer+2, 7); memcpy(bkgLocBuf[0]+2, rfRxBuffer+2, 7);
} }
if(rfRxBuffer[0]=='P') memcpy(bkgLocBuf[1], rfRxBuffer, 9); if (rfRxBuffer[0]=='P') memcpy(bkgLocBuf[1], rfRxBuffer, 9);
if(rfRxBuffer[0]=='G') memcpy(bkgLocBuf[2], rfRxBuffer, 9); if (rfRxBuffer[0]=='G') memcpy(bkgLocBuf[2], rfRxBuffer, 9);
dataReady = 0; dataReady = 0;
} }
} }
@ -665,7 +665,7 @@ static void bindChannels(const uint8_t* RF_HEAD, uint8_t* hop_lst)
for (int j=0; j<4; j++) { for (int j=0; j<4; j++) {
for (int i=0; i<4; i++) { for (int i=0; i<4; i++) {
n = RF_HEAD[i]%128; n = RF_HEAD[i]%128;
if(j==3) n /= 5; if (j==3) n /= 5;
else n /= j+1; else n /= j+1;
hop_lst[4*j+i] = checkChannel(n,hop_lst); hop_lst[4*j+i] = checkChannel(n,hop_lst);
} }
@ -710,17 +710,17 @@ uint8_t eleresBind(void)
bindChannels(eleresSignaturePtr,holList); bindChannels(eleresSignaturePtr,holList);
channelHoppingTime = 33; channelHoppingTime = 33;
RED_LED_OFF; RED_LED_OFF;
while(timeout--) { while (timeout--) {
eleresDataReceived(NULL); eleresDataReceived(NULL);
eleresSetRcDataFromPayload(NULL,NULL); eleresSetRcDataFromPayload(NULL,NULL);
if (rfRxBuffer[0]==0x42) { if (rfRxBuffer[0]==0x42) {
for(i=0; i<4; i++) { for (i=0; i<4; i++) {
if (rfRxBuffer[i+1]==eleres_signature_old[i]) eleres_signature_OK_count++; if (rfRxBuffer[i+1]==eleres_signature_old[i]) eleres_signature_OK_count++;
else eleres_signature_OK_count = 0; else eleres_signature_OK_count = 0;
} }
for(i=0; i<4; i++) eleres_signature_old[i] = rfRxBuffer[i+1]; for (i=0; i<4; i++) eleres_signature_old[i] = rfRxBuffer[i+1];
if (eleres_signature_OK_count>200) { if (eleres_signature_OK_count>200) {
for(i=0; i<4; i++) for (i=0; i<4; i++)
eleresSignaturePtr[i] = eleres_signature_old[i]; eleresSignaturePtr[i] = eleres_signature_old[i];
RED_LED_OFF; RED_LED_OFF;
saveConfigAndNotify(); saveConfigAndNotify();

View file

@ -316,7 +316,7 @@ static void jetiExBusDataReceive(uint16_t c)
// Check if we shall start a frame? // Check if we shall start a frame?
if (jetiExBusFramePosition == 0) { if (jetiExBusFramePosition == 0) {
switch(c){ switch (c){
case EXBUS_START_CHANNEL_FRAME: case EXBUS_START_CHANNEL_FRAME:
jetiExBusFrameState = EXBUS_STATE_IN_PROGRESS; jetiExBusFrameState = EXBUS_STATE_IN_PROGRESS;
jetiExBusFrame = jetiExBusChannelFrame; jetiExBusFrame = jetiExBusChannelFrame;
@ -339,12 +339,12 @@ static void jetiExBusDataReceive(uint16_t c)
// Check the header for the message length // Check the header for the message length
if (jetiExBusFramePosition == EXBUS_HEADER_LEN) { if (jetiExBusFramePosition == EXBUS_HEADER_LEN) {
if((jetiExBusFrameState == EXBUS_STATE_IN_PROGRESS) && (jetiExBusFrame[EXBUS_HEADER_MSG_LEN] <= EXBUS_MAX_CHANNEL_FRAME_SIZE)) { if ((jetiExBusFrameState == EXBUS_STATE_IN_PROGRESS) && (jetiExBusFrame[EXBUS_HEADER_MSG_LEN] <= EXBUS_MAX_CHANNEL_FRAME_SIZE)) {
jetiExBusFrameLength = jetiExBusFrame[EXBUS_HEADER_MSG_LEN]; jetiExBusFrameLength = jetiExBusFrame[EXBUS_HEADER_MSG_LEN];
return; return;
} }
if((jetiExBusRequestState == EXBUS_STATE_IN_PROGRESS) && (jetiExBusFrame[EXBUS_HEADER_MSG_LEN] <= EXBUS_MAX_REQUEST_FRAME_SIZE)) { if ((jetiExBusRequestState == EXBUS_STATE_IN_PROGRESS) && (jetiExBusFrame[EXBUS_HEADER_MSG_LEN] <= EXBUS_MAX_REQUEST_FRAME_SIZE)) {
jetiExBusFrameLength = jetiExBusFrame[EXBUS_HEADER_MSG_LEN]; jetiExBusFrameLength = jetiExBusFrame[EXBUS_HEADER_MSG_LEN];
return; return;
} }
@ -375,7 +375,7 @@ uint8_t jetiExBusFrameStatus()
if (jetiExBusFrameState != EXBUS_STATE_RECEIVED) if (jetiExBusFrameState != EXBUS_STATE_RECEIVED)
return RX_FRAME_PENDING; return RX_FRAME_PENDING;
if(calcCRC16(jetiExBusChannelFrame, jetiExBusChannelFrame[EXBUS_HEADER_MSG_LEN]) == 0) { if (calcCRC16(jetiExBusChannelFrame, jetiExBusChannelFrame[EXBUS_HEADER_MSG_LEN]) == 0) {
jetiExBusDecodeChannelFrame(jetiExBusChannelFrame); jetiExBusDecodeChannelFrame(jetiExBusChannelFrame);
jetiExBusFrameState = EXBUS_STATE_ZERO; jetiExBusFrameState = EXBUS_STATE_ZERO;
return RX_FRAME_COMPLETE; return RX_FRAME_COMPLETE;
@ -448,18 +448,18 @@ uint8_t createExTelemetrieValueMessage(uint8_t *exMessage, uint8_t itemStart)
if ((item & 0x0F) == 0) if ((item & 0x0F) == 0)
item++; item++;
if(item >= JETI_EX_SENSOR_COUNT) if (item >= JETI_EX_SENSOR_COUNT)
item = 1; item = 1;
exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0; // Device ID exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0; // Device ID
uint8_t *p = &exMessage[EXTEL_HEADER_ID]; uint8_t *p = &exMessage[EXTEL_HEADER_ID];
while(item <= (itemStart | 0x0F)) { while (item <= (itemStart | 0x0F)) {
*p++ = ((item & 0x0F) << 4) | jetiExSensors[item].exDataType; // Sensor ID (%16) | EX Data Type *p++ = ((item & 0x0F) << 4) | jetiExSensors[item].exDataType; // Sensor ID (%16) | EX Data Type
sensorValue = jetiExSensors[item].value; sensorValue = jetiExSensors[item].value;
iCount = exDataTypeLen[jetiExSensors[item].exDataType]; iCount = exDataTypeLen[jetiExSensors[item].exDataType];
while(iCount > 1) { while (iCount > 1) {
*p++ = sensorValue; *p++ = sensorValue;
sensorValue = sensorValue >> 8; sensorValue = sensorValue >> 8;
iCount--; iCount--;
@ -467,9 +467,9 @@ uint8_t createExTelemetrieValueMessage(uint8_t *exMessage, uint8_t itemStart)
*p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals; *p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals;
item++; item++;
if(item >= JETI_EX_SENSOR_COUNT) if (item >= JETI_EX_SENSOR_COUNT)
break; break;
if(EXTEL_MAX_PAYLOAD <= ((p-&exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[item].exDataType]) + 1) if (EXTEL_MAX_PAYLOAD <= ((p-&exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[item].exDataType]) + 1)
break; break;
} }
@ -512,13 +512,13 @@ void handleJetiExBusTelemetry(void)
// to prevent timing issues from request to answer - max. 4ms // to prevent timing issues from request to answer - max. 4ms
timeDiff = micros() - jetiTimeStampRequest; timeDiff = micros() - jetiTimeStampRequest;
if(timeDiff > 3000) { // include reserved time if (timeDiff > 3000) { // include reserved time
jetiExBusRequestState = EXBUS_STATE_ZERO; jetiExBusRequestState = EXBUS_STATE_ZERO;
framesLost++; framesLost++;
return; return;
} }
if((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) { if ((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) {
jetiExSensors[EX_VOLTAGE].value = vbat; jetiExSensors[EX_VOLTAGE].value = vbat;
jetiExSensors[EX_CURRENT].value = amperage; jetiExSensors[EX_CURRENT].value = amperage;
jetiExSensors[EX_ALTITUDE].value = baro.BaroAlt; jetiExSensors[EX_ALTITUDE].value = baro.BaroAlt;

View file

@ -152,7 +152,7 @@ static void decode_bind_packet(uint8_t *packet)
// Returns whether the data was successfully decoded // Returns whether the data was successfully decoded
static rx_spi_received_e decode_packet(uint8_t *packet) static rx_spi_received_e decode_packet(uint8_t *packet)
{ {
if(bind_phase != PHASE_BOUND) { if (bind_phase != PHASE_BOUND) {
decode_bind_packet(packet); decode_bind_packet(packet);
return RX_SPI_RECEIVED_BIND; return RX_SPI_RECEIVED_BIND;
} }

View file

@ -126,11 +126,11 @@ uint8_t spektrumFrameStatus(void)
// This is the first frame status received. // This is the first frame status received.
spek_fade_last_sec_count = fade; spek_fade_last_sec_count = fade;
spek_fade_last_sec = current_secs; spek_fade_last_sec = current_secs;
} else if(spek_fade_last_sec != current_secs) { } else if (spek_fade_last_sec != current_secs) {
// If the difference is > 1, then we missed several seconds worth of frames and // If the difference is > 1, then we missed several seconds worth of frames and
// should just throw out the fade calc (as it's likely a full signal loss). // should just throw out the fade calc (as it's likely a full signal loss).
if((current_secs - spek_fade_last_sec) == 1) { if ((current_secs - spek_fade_last_sec) == 1) {
if(rssi_channel != 0) { if (rssi_channel != 0) {
if (spekHiRes) if (spekHiRes)
spekChannelData[rssi_channel] = 2048 - ((fade - spek_fade_last_sec_count) * 2048 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC)); spekChannelData[rssi_channel] = 2048 - ((fade - spek_fade_last_sec_count) * 2048 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
else else
@ -145,7 +145,7 @@ uint8_t spektrumFrameStatus(void)
for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) { for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) {
const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift); const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) { if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) {
if(rssi_channel == 0 || spekChannel != rssi_channel) { if (rssi_channel == 0 || spekChannel != rssi_channel) {
spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b]; spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
} }
} }

View file

@ -130,7 +130,7 @@ void batteryUpdate(uint32_t vbatTimeDelta)
batteryCriticalVoltage = 0; batteryCriticalVoltage = 0;
} }
switch(batteryState) switch (batteryState)
{ {
case BATTERY_OK: case BATTERY_OK:
if (vbat <= (batteryWarningVoltage - VBATT_HYSTERESIS)) { if (vbat <= (batteryWarningVoltage - VBATT_HYSTERESIS)) {
@ -192,7 +192,7 @@ void currentMeterUpdate(int32_t lastUpdateAt)
int32_t throttleFactor = 0; int32_t throttleFactor = 0;
int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
switch(batteryConfig()->currentMeterType) { switch (batteryConfig()->currentMeterType) {
case CURRENT_SENSOR_ADC: case CURRENT_SENSOR_ADC:
amperageRaw -= amperageRaw / 8; amperageRaw -= amperageRaw / 8;
amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT)); amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT));

View file

@ -109,7 +109,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
dev->magAlign = ALIGN_DEFAULT; dev->magAlign = ALIGN_DEFAULT;
switch(magHardwareToUse) { switch (magHardwareToUse) {
case MAG_AUTODETECT: case MAG_AUTODETECT:
case MAG_HMC5883: case MAG_HMC5883:
#ifdef USE_MAG_HMC5883 #ifdef USE_MAG_HMC5883

View file

@ -153,7 +153,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
{ {
dev->gyroAlign = ALIGN_DEFAULT; dev->gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) { switch (gyroHardware) {
case GYRO_AUTODETECT: case GYRO_AUTODETECT:
// fallthrough // fallthrough

View file

@ -304,7 +304,7 @@ void SetSysClock(void)
{ {
HSEStatus = RCC->CR & RCC_CR_HSERDY; HSEStatus = RCC->CR & RCC_CR_HSERDY;
StartUpCounter++; StartUpCounter++;
} while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); } while ((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT));
if ((RCC->CR & RCC_CR_HSERDY) != RESET) if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{ {
@ -343,7 +343,7 @@ void SetSysClock(void)
RCC->CR |= RCC_CR_PLLON; RCC->CR |= RCC_CR_PLLON;
/* Wait till PLL is ready */ /* Wait till PLL is ready */
while((RCC->CR & RCC_CR_PLLRDY) == 0) while ((RCC->CR & RCC_CR_PLLRDY) == 0)
{ {
} }

View file

@ -167,16 +167,16 @@
RCC_OscInitStruct.PLL.PLLQ = PLL_Q; RCC_OscInitStruct.PLL.PLLQ = PLL_Q;
ret = HAL_RCC_OscConfig(&RCC_OscInitStruct); ret = HAL_RCC_OscConfig(&RCC_OscInitStruct);
if(ret != HAL_OK) if (ret != HAL_OK)
{ {
while(1) { ; } while (1) { ; }
} }
/* Activate the OverDrive to reach the 216 MHz Frequency */ /* Activate the OverDrive to reach the 216 MHz Frequency */
ret = HAL_PWREx_EnableOverDrive(); ret = HAL_PWREx_EnableOverDrive();
if(ret != HAL_OK) if (ret != HAL_OK)
{ {
while(1) { ; } while (1) { ; }
} }
/* Select PLLSAI output as USB clock source */ /* Select PLLSAI output as USB clock source */
PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_CLK48; PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_CLK48;
@ -184,9 +184,9 @@
PeriphClkInitStruct.PLLSAI.PLLSAIN = PLL_SAIN; PeriphClkInitStruct.PLLSAI.PLLSAIN = PLL_SAIN;
PeriphClkInitStruct.PLLSAI.PLLSAIQ = PLL_SAIQ; PeriphClkInitStruct.PLLSAI.PLLSAIQ = PLL_SAIQ;
PeriphClkInitStruct.PLLSAI.PLLSAIP = PLL_SAIP; PeriphClkInitStruct.PLLSAI.PLLSAIP = PLL_SAIP;
if(HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
{ {
while(1) {}; while (1) {};
} }
/* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 clocks dividers */ /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 clocks dividers */
@ -197,9 +197,9 @@
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
ret = HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_7); ret = HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_7);
if(ret != HAL_OK) if (ret != HAL_OK)
{ {
while(1) { ; } while (1) { ; }
} }
PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART1|RCC_PERIPHCLK_USART2 PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART1|RCC_PERIPHCLK_USART2
@ -223,7 +223,7 @@
ret = HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct); ret = HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct);
if (ret != HAL_OK) if (ret != HAL_OK)
{ {
while(1) { ; } while (1) { ; }
} }
// Activating the timerprescalers while the APBx prescalers are 1/2/4 will connect the TIMxCLK to HCLK which has been configured to 216MHz // Activating the timerprescalers while the APBx prescalers are 1/2/4 will connect the TIMxCLK to HCLK which has been configured to 216MHz
@ -288,9 +288,9 @@ void SystemInit(void)
/* Configure the system clock to 216 MHz */ /* Configure the system clock to 216 MHz */
SystemClock_Config(); SystemClock_Config();
if(SystemCoreClock != 216000000) if (SystemCoreClock != 216000000)
{ {
while(1) while (1)
{ {
// There is a mismatch between the configured clock and the expected clock in portable.h // There is a mismatch between the configured clock and the expected clock in portable.h
} }

View file

@ -431,7 +431,7 @@ void configureLtmTelemetryPort(void)
} }
/* setup scheduler, default to 'normal' */ /* setup scheduler, default to 'normal' */
if(telemetryConfig()->ltmUpdateRate == LTM_RATE_MEDIUM) if (telemetryConfig()->ltmUpdateRate == LTM_RATE_MEDIUM)
ltm_schedule = ltm_medium_schedule; ltm_schedule = ltm_medium_schedule;
else if (telemetryConfig()->ltmUpdateRate == LTM_RATE_SLOW) else if (telemetryConfig()->ltmUpdateRate == LTM_RATE_SLOW)
ltm_schedule = ltm_slow_schedule; ltm_schedule = ltm_slow_schedule;
@ -439,9 +439,9 @@ void configureLtmTelemetryPort(void)
ltm_schedule = ltm_normal_schedule; ltm_schedule = ltm_normal_schedule;
/* Sanity check that we can support the scheduler */ /* Sanity check that we can support the scheduler */
if(baudRateIndex == BAUD_2400 && telemetryConfig()->ltmUpdateRate == LTM_RATE_NORMAL) if (baudRateIndex == BAUD_2400 && telemetryConfig()->ltmUpdateRate == LTM_RATE_NORMAL)
ltm_schedule = ltm_medium_schedule; ltm_schedule = ltm_medium_schedule;
if(baudRateIndex == BAUD_1200) if (baudRateIndex == BAUD_1200)
ltm_schedule = ltm_slow_schedule; ltm_schedule = ltm_slow_schedule;
ltmPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_LTM, NULL, baudRates[baudRateIndex], TELEMETRY_LTM_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); ltmPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_LTM, NULL, baudRates[baudRateIndex], TELEMETRY_LTM_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);

View file

@ -424,7 +424,7 @@ void mavlinkSendHUDAndHeartbeat(void)
mavModes |= MAV_MODE_FLAG_SAFETY_ARMED; mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
uint8_t mavSystemType; uint8_t mavSystemType;
switch(mixerConfig()->mixerMode) switch (mixerConfig()->mixerMode)
{ {
case MIXER_TRI: case MIXER_TRI:
mavSystemType = MAV_TYPE_TRICOPTER; mavSystemType = MAV_TYPE_TRICOPTER;

View file

@ -303,7 +303,7 @@ void handleSmartPortTelemetry(void)
int32_t tmpi; int32_t tmpi;
switch(id) { switch (id) {
#ifdef GPS #ifdef GPS
case FSSP_DATAID_SPEED : case FSSP_DATAID_SPEED :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {

View file

@ -193,7 +193,7 @@ void USB_Istr(void)
_SetCNTR(wCNTR); _SetCNTR(wCNTR);
/*poll for RESET flag in ISTR*/ /*poll for RESET flag in ISTR*/
while((_GetISTR()&ISTR_RESET) == 0); while ((_GetISTR()&ISTR_RESET) == 0);
/* clear RESET flag in ISTR */ /* clear RESET flag in ISTR */
_SetISTR((uint16_t)CLR_RESET); _SetISTR((uint16_t)CLR_RESET);

View file

@ -123,7 +123,7 @@ static int8_t CDC_Itf_Init(void)
/*##-4- Start the TIM Base generation in interrupt mode ####################*/ /*##-4- Start the TIM Base generation in interrupt mode ####################*/
/* Start Channel1 */ /* Start Channel1 */
if(HAL_TIM_Base_Start_IT(&TimHandle) != HAL_OK) if (HAL_TIM_Base_Start_IT(&TimHandle) != HAL_OK)
{ {
/* Starting Error */ /* Starting Error */
Error_Handler(); Error_Handler();
@ -222,14 +222,14 @@ static int8_t CDC_Itf_Control (uint8_t cmd, uint8_t* pbuf, uint16_t length)
*/ */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{ {
if(htim->Instance != TIMusb) return; if (htim->Instance != TIMusb) return;
uint32_t buffptr; uint32_t buffptr;
uint32_t buffsize; uint32_t buffsize;
if(UserTxBufPtrOut != UserTxBufPtrIn) if (UserTxBufPtrOut != UserTxBufPtrIn)
{ {
if(UserTxBufPtrOut > UserTxBufPtrIn) /* Roll-back */ if (UserTxBufPtrOut > UserTxBufPtrIn) /* Roll-back */
{ {
buffsize = APP_RX_DATA_SIZE - UserTxBufPtrOut; buffsize = APP_RX_DATA_SIZE - UserTxBufPtrOut;
} }
@ -242,7 +242,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
USBD_CDC_SetTxBuffer(&USBD_Device, (uint8_t*)&UserTxBuffer[buffptr], buffsize); USBD_CDC_SetTxBuffer(&USBD_Device, (uint8_t*)&UserTxBuffer[buffptr], buffsize);
if(USBD_CDC_TransmitPacket(&USBD_Device) == USBD_OK) if (USBD_CDC_TransmitPacket(&USBD_Device) == USBD_OK)
{ {
UserTxBufPtrOut += buffsize; UserTxBufPtrOut += buffsize;
if (UserTxBufPtrOut == APP_TX_DATA_SIZE) if (UserTxBufPtrOut == APP_TX_DATA_SIZE)
@ -288,7 +288,7 @@ static void TIM_Config(void)
TimHandle.Init.Prescaler = (SystemCoreClock / 2 / (1000000)) - 1; TimHandle.Init.Prescaler = (SystemCoreClock / 2 / (1000000)) - 1;
TimHandle.Init.ClockDivision = 0; TimHandle.Init.ClockDivision = 0;
TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
if(HAL_TIM_Base_Init(&TimHandle) != HAL_OK) if (HAL_TIM_Base_Init(&TimHandle) != HAL_OK)
{ {
/* Initialization Error */ /* Initialization Error */
Error_Handler(); Error_Handler();
@ -318,7 +318,7 @@ static void Error_Handler(void)
uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len)
{ {
uint32_t count = 0; uint32_t count = 0;
if( (rxBuffPtr != NULL)) if ( (rxBuffPtr != NULL))
{ {
while ((rxAvailable > 0) && count < len) while ((rxAvailable > 0) && count < len)
{ {
@ -326,7 +326,7 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len)
rxBuffPtr++; rxBuffPtr++;
rxAvailable--; rxAvailable--;
count++; count++;
if(rxAvailable < 1) if (rxAvailable < 1)
USBD_CDC_ReceivePacket(&USBD_Device); USBD_CDC_ReceivePacket(&USBD_Device);
} }
} }
@ -361,7 +361,7 @@ uint32_t CDC_Send_FreeBytes(void)
uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength) uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength)
{ {
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*)USBD_Device.pClassData; USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*)USBD_Device.pClassData;
while(hcdc->TxState != 0); while (hcdc->TxState != 0);
for (uint32_t i = 0; i < sendLength; i++) for (uint32_t i = 0; i < sendLength; i++)
{ {

View file

@ -84,7 +84,7 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd)
{ {
GPIO_InitTypeDef GPIO_InitStruct; GPIO_InitTypeDef GPIO_InitStruct;
if(hpcd->Instance == USB_OTG_FS) if (hpcd->Instance == USB_OTG_FS)
{ {
/* Configure USB FS GPIOs */ /* Configure USB FS GPIOs */
__HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE();
@ -97,7 +97,7 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd)
GPIO_InitStruct.Alternate = GPIO_AF10_OTG_FS; GPIO_InitStruct.Alternate = GPIO_AF10_OTG_FS;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
if(hpcd->Init.vbus_sensing_enable == 1) if (hpcd->Init.vbus_sensing_enable == 1)
{ {
/* Configure VBUS Pin */ /* Configure VBUS Pin */
GPIO_InitStruct.Pin = GPIO_PIN_9; GPIO_InitStruct.Pin = GPIO_PIN_9;
@ -122,7 +122,7 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd)
/* Enable USBFS Interrupt */ /* Enable USBFS Interrupt */
HAL_NVIC_EnableIRQ(OTG_FS_IRQn); HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
} }
else if(hpcd->Instance == USB_OTG_HS) else if (hpcd->Instance == USB_OTG_HS)
{ {
#ifdef USE_USB_HS_IN_FS #ifdef USE_USB_HS_IN_FS
@ -136,7 +136,7 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd)
GPIO_InitStruct.Alternate = GPIO_AF12_OTG_HS_FS; GPIO_InitStruct.Alternate = GPIO_AF12_OTG_HS_FS;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
if(hpcd->Init.vbus_sensing_enable == 1) if (hpcd->Init.vbus_sensing_enable == 1)
{ {
/* Configure VBUS Pin */ /* Configure VBUS Pin */
GPIO_InitStruct.Pin = GPIO_PIN_13 ; GPIO_InitStruct.Pin = GPIO_PIN_13 ;
@ -216,13 +216,13 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd)
*/ */
void HAL_PCD_MspDeInit(PCD_HandleTypeDef *hpcd) void HAL_PCD_MspDeInit(PCD_HandleTypeDef *hpcd)
{ {
if(hpcd->Instance == USB_OTG_FS) if (hpcd->Instance == USB_OTG_FS)
{ {
/* Disable USB FS Clock */ /* Disable USB FS Clock */
__HAL_RCC_USB_OTG_FS_CLK_DISABLE(); __HAL_RCC_USB_OTG_FS_CLK_DISABLE();
__HAL_RCC_SYSCFG_CLK_DISABLE(); __HAL_RCC_SYSCFG_CLK_DISABLE();
} }
else if(hpcd->Instance == USB_OTG_HS) else if (hpcd->Instance == USB_OTG_HS)
{ {
/* Disable USB HS Clocks */ /* Disable USB HS Clocks */
__HAL_RCC_USB_OTG_HS_CLK_DISABLE(); __HAL_RCC_USB_OTG_HS_CLK_DISABLE();
@ -286,7 +286,7 @@ void HAL_PCD_ResetCallback(PCD_HandleTypeDef *hpcd)
USBD_SpeedTypeDef speed = USBD_SPEED_FULL; USBD_SpeedTypeDef speed = USBD_SPEED_FULL;
/* Set USB Current Speed */ /* Set USB Current Speed */
switch(hpcd->Init.speed) switch (hpcd->Init.speed)
{ {
case PCD_SPEED_HIGH: case PCD_SPEED_HIGH:
speed = USBD_SPEED_HIGH; speed = USBD_SPEED_HIGH;
@ -559,7 +559,7 @@ uint8_t USBD_LL_IsStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr)
{ {
PCD_HandleTypeDef *hpcd = pdev->pData; PCD_HandleTypeDef *hpcd = pdev->pData;
if((ep_addr & 0x80) == 0x80) if ((ep_addr & 0x80) == 0x80)
{ {
return hpcd->IN_ep[ep_addr & 0x7F].is_stall; return hpcd->IN_ep[ep_addr & 0x7F].is_stall;
} }

View file

@ -174,7 +174,7 @@ uint8_t *USBD_VCP_LangIDStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
*/ */
uint8_t *USBD_VCP_ProductStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) uint8_t *USBD_VCP_ProductStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
{ {
if(speed == USBD_SPEED_HIGH) if (speed == USBD_SPEED_HIGH)
{ {
USBD_GetString((uint8_t *)USBD_PRODUCT_HS_STRING, USBD_StrDesc, length); USBD_GetString((uint8_t *)USBD_PRODUCT_HS_STRING, USBD_StrDesc, length);
} }
@ -223,7 +223,7 @@ uint8_t *USBD_VCP_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
*/ */
uint8_t *USBD_VCP_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) uint8_t *USBD_VCP_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
{ {
if(speed == USBD_SPEED_HIGH) if (speed == USBD_SPEED_HIGH)
{ {
USBD_GetString((uint8_t *)USBD_CONFIGURATION_HS_STRING, USBD_StrDesc, length); USBD_GetString((uint8_t *)USBD_CONFIGURATION_HS_STRING, USBD_StrDesc, length);
} }
@ -242,7 +242,7 @@ uint8_t *USBD_VCP_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
*/ */
uint8_t *USBD_VCP_InterfaceStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) uint8_t *USBD_VCP_InterfaceStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
{ {
if(speed == USBD_SPEED_HIGH) if (speed == USBD_SPEED_HIGH)
{ {
USBD_GetString((uint8_t *)USBD_INTERFACE_HS_STRING, USBD_StrDesc, length); USBD_GetString((uint8_t *)USBD_INTERFACE_HS_STRING, USBD_StrDesc, length);
} }
@ -286,9 +286,9 @@ static void IntToUnicode (uint32_t value , uint8_t *pbuf , uint8_t len)
{ {
uint8_t idx = 0; uint8_t idx = 0;
for( idx = 0; idx < len; idx ++) for ( idx = 0; idx < len; idx ++)
{ {
if( ((value >> 28)) < 0xA ) if ( ((value >> 28)) < 0xA )
{ {
pbuf[ 2* idx] = (value >> 28) + '0'; pbuf[ 2* idx] = (value >> 28) + '0';
} }

View file

@ -59,7 +59,7 @@ void PendSV_Handler(void)
#ifdef USE_USB_OTG_FS #ifdef USE_USB_OTG_FS
void OTG_FS_WKUP_IRQHandler(void) void OTG_FS_WKUP_IRQHandler(void)
{ {
if(USB_OTG_dev.cfg.low_power) if (USB_OTG_dev.cfg.low_power)
{ {
*(uint32_t *)(0xE000ED10) &= 0xFFFFFFF9 ; *(uint32_t *)(0xE000ED10) &= 0xFFFFFFF9 ;
SystemInit(); SystemInit();
@ -77,7 +77,7 @@ void OTG_FS_WKUP_IRQHandler(void)
#ifdef USE_USB_OTG_HS #ifdef USE_USB_OTG_HS
void OTG_HS_WKUP_IRQHandler(void) void OTG_HS_WKUP_IRQHandler(void)
{ {
if(USB_OTG_dev.cfg.low_power) if (USB_OTG_dev.cfg.low_power)
{ {
*(uint32_t *)(0xE000ED10) &= 0xFFFFFFF9 ; *(uint32_t *)(0xE000ED10) &= 0xFFFFFFF9 ;
SystemInit(); SystemInit();

View file

@ -225,7 +225,7 @@ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length)
{ {
if(speed == 0) if (speed == 0)
USBD_GetString ((uint8_t*)USBD_PRODUCT_HS_STRING, USBD_StrDesc, length); USBD_GetString ((uint8_t*)USBD_PRODUCT_HS_STRING, USBD_StrDesc, length);
else else
USBD_GetString ((uint8_t*)USBD_PRODUCT_FS_STRING, USBD_StrDesc, length); USBD_GetString ((uint8_t*)USBD_PRODUCT_FS_STRING, USBD_StrDesc, length);
@ -256,7 +256,7 @@ uint8_t * USBD_USR_ManufacturerStrDescriptor( uint8_t speed , uint16_t *length)
*/ */
uint8_t * USBD_USR_SerialStrDescriptor( uint8_t speed , uint16_t *length) uint8_t * USBD_USR_SerialStrDescriptor( uint8_t speed , uint16_t *length)
{ {
if(speed == USB_OTG_SPEED_HIGH) if (speed == USB_OTG_SPEED_HIGH)
USBD_GetString ((uint8_t*)USBD_SERIALNUMBER_HS_STRING, USBD_StrDesc, length); USBD_GetString ((uint8_t*)USBD_SERIALNUMBER_HS_STRING, USBD_StrDesc, length);
else else
USBD_GetString ((uint8_t*)USBD_SERIALNUMBER_FS_STRING, USBD_StrDesc, length); USBD_GetString ((uint8_t*)USBD_SERIALNUMBER_FS_STRING, USBD_StrDesc, length);
@ -273,7 +273,7 @@ uint8_t * USBD_USR_SerialStrDescriptor( uint8_t speed , uint16_t *length)
*/ */
uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length) uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length)
{ {
if(speed == USB_OTG_SPEED_HIGH) if (speed == USB_OTG_SPEED_HIGH)
USBD_GetString ((uint8_t*)USBD_CONFIGURATION_HS_STRING, USBD_StrDesc, length); USBD_GetString ((uint8_t*)USBD_CONFIGURATION_HS_STRING, USBD_StrDesc, length);
else else
USBD_GetString ((uint8_t*)USBD_CONFIGURATION_FS_STRING, USBD_StrDesc, length); USBD_GetString ((uint8_t*)USBD_CONFIGURATION_FS_STRING, USBD_StrDesc, length);
@ -291,7 +291,7 @@ uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length)
*/ */
uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length) uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length)
{ {
if(speed == 0) if (speed == 0)
USBD_GetString ((uint8_t*)USBD_INTERFACE_HS_STRING, USBD_StrDesc, length); USBD_GetString ((uint8_t*)USBD_INTERFACE_HS_STRING, USBD_StrDesc, length);
else else
USBD_GetString ((uint8_t*)USBD_INTERFACE_FS_STRING, USBD_StrDesc, length); USBD_GetString ((uint8_t*)USBD_INTERFACE_FS_STRING, USBD_StrDesc, length);