1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 11:59:58 +03:00

Whitespace tidy

This commit is contained in:
Martin Budden 2017-07-05 06:36:22 +01:00
parent ee8763bbf1
commit 3d4f0bb137
97 changed files with 555 additions and 555 deletions

View file

@ -906,7 +906,7 @@ bool startedLoggingInTestMode = false;
void startInTestMode(void) void startInTestMode(void)
{ {
if(!startedLoggingInTestMode) { if (!startedLoggingInTestMode) {
if (blackboxConfig()->device == BLACKBOX_DEVICE_SERIAL) { if (blackboxConfig()->device == BLACKBOX_DEVICE_SERIAL) {
serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
if (sharedBlackboxAndMspPort) { if (sharedBlackboxAndMspPort) {
@ -919,7 +919,7 @@ void startInTestMode(void)
} }
void stopInTestMode(void) void stopInTestMode(void)
{ {
if(startedLoggingInTestMode) { if (startedLoggingInTestMode) {
blackboxFinish(); blackboxFinish();
startedLoggingInTestMode = false; startedLoggingInTestMode = false;
} }
@ -954,7 +954,7 @@ bool inMotorTestMode(void) {
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
atLeastOneMotorActivated |= (motor_disarmed[i] != inactiveMotorCommand); atLeastOneMotorActivated |= (motor_disarmed[i] != inactiveMotorCommand);
if(atLeastOneMotorActivated) { if (atLeastOneMotorActivated) {
resetTime = millis() + 5000; // add 5 seconds resetTime = millis() + 5000; // add 5 seconds
return true; return true;
} else { } else {
@ -1638,7 +1638,7 @@ void blackboxUpdate(timeUs_t currentTimeUs)
blackboxSetState(BLACKBOX_STATE_ERASED); blackboxSetState(BLACKBOX_STATE_ERASED);
beeper(BEEPER_BLACKBOX_ERASE); beeper(BEEPER_BLACKBOX_ERASE);
} }
break; break;
case BLACKBOX_STATE_ERASED: case BLACKBOX_STATE_ERASED:
if (!IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) { if (!IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) {
blackboxSetState(BLACKBOX_STATE_STOPPED); blackboxSetState(BLACKBOX_STATE_STOPPED);
@ -1663,13 +1663,13 @@ void blackboxUpdate(timeUs_t currentTimeUs)
} }
#endif #endif
} else { // Only log in test mode if there is room! } else { // Only log in test mode if there is room!
if(blackboxConfig()->on_motor_test) { if (blackboxConfig()->on_motor_test) {
// Handle Motor Test Mode // Handle Motor Test Mode
if(inMotorTestMode()) { if (inMotorTestMode()) {
if(blackboxState==BLACKBOX_STATE_STOPPED) if (blackboxState==BLACKBOX_STATE_STOPPED)
startInTestMode(); startInTestMode();
} else { } else {
if(blackboxState!=BLACKBOX_STATE_STOPPED) if (blackboxState!=BLACKBOX_STATE_STOPPED)
stopInTestMode(); stopInTestMode();
} }
} }

View file

@ -221,7 +221,7 @@ bool blackboxDeviceOpen(void)
*/ */
switch(baudRateIndex) { switch (baudRateIndex) {
case BAUD_1000000: case BAUD_1000000:
case BAUD_1500000: case BAUD_1500000:
case BAUD_2000000: case BAUD_2000000:

View file

@ -606,7 +606,7 @@ static void cmsTraverseGlobalExit(const CMS_Menu *pMenu)
} }
long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
{ {
int exitType = (int)ptr; int exitType = (int)ptr;
switch (exitType) { switch (exitType) {
case CMS_EXIT_SAVE: case CMS_EXIT_SAVE:

View file

@ -154,11 +154,11 @@ float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
{ {
/* compute result */ /* compute result */
const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2; const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2;
/* shift x1 to x2, input to x1 */ /* shift x1 to x2, input to x1 */
filter->x2 = filter->x1; filter->x2 = filter->x1;
filter->x1 = input; filter->x1 = input;
/* shift y1 to y2, result to y1 */ /* shift y1 to y2, result to y1 */
filter->y2 = filter->y1; filter->y2 = filter->y1;
filter->y1 = result; filter->y1 = result;

View file

@ -235,7 +235,7 @@ static int write_word(config_streamer_t *c, uint32_t value)
EraseInitStruct.Sector = getFLASHSectorForEEPROM(); EraseInitStruct.Sector = getFLASHSectorForEEPROM();
uint32_t SECTORError; uint32_t SECTORError;
const HAL_StatusTypeDef status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError); const HAL_StatusTypeDef status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
if (status != HAL_OK){ if (status != HAL_OK) {
return -1; return -1;
} }
} }

View file

@ -87,7 +87,7 @@ extern const uint8_t __pg_resetdata_end[];
do { \ do { \
extern const pgRegistry_t _name ##_Registry; \ extern const pgRegistry_t _name ##_Registry; \
pgReset(&_name ## _Registry); \ pgReset(&_name ## _Registry); \
} while(0) \ } while (0) \
/**/ /**/
// Declare system config // Declare system config

View file

@ -85,7 +85,7 @@ static inline void mma8451ConfigureInterrupt(void)
#ifdef MMA8451_INT_PIN #ifdef MMA8451_INT_PIN
IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_MPU_EXTI, 0); IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_MPU_EXTI, 0);
// TODO - maybe pullup / pulldown ? // TODO - maybe pullup / pulldown ?
IOConfigGPIO(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), IOCFG_IN_FLOATING); IOConfigGPIO(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), IOCFG_IN_FLOATING);
#endif #endif
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH) i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)

View file

@ -110,7 +110,7 @@ bool bmi160Detect(const busDevice_t *bus)
delay(10); // Give SPI some time to start up delay(10); // Give SPI some time to start up
/* Check the chip ID */ /* Check the chip ID */
if (spiReadRegister(bus, BMI160_REG_CHIPID) != 0xd1){ if (spiReadRegister(bus, BMI160_REG_CHIPID) != 0xd1) {
return false; return false;
} }
@ -130,7 +130,7 @@ static void BMI160_Init(const busDevice_t *bus)
} }
/* Configure the BMI160 Sensor */ /* Configure the BMI160 Sensor */
if (BMI160_Config(bus) != 0){ if (BMI160_Config(bus) != 0) {
return; return;
} }
@ -152,12 +152,12 @@ static int32_t BMI160_Config(const busDevice_t *bus)
{ {
// Set normal power mode for gyro and accelerometer // Set normal power mode for gyro and accelerometer
if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL) != 0){ if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL) != 0) {
return -1; return -1;
} }
delay(100); // can take up to 80ms delay(100); // can take up to 80ms
if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL) != 0){ if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL) != 0) {
return -2; return -2;
} }
delay(5); // can take up to 3.8ms delay(5); // can take up to 3.8ms
@ -170,47 +170,47 @@ static int32_t BMI160_Config(const busDevice_t *bus)
// Set odr and ranges // Set odr and ranges
// Set acc_us = 0 acc_bwp = 0b010 so only the first filter stage is used // Set acc_us = 0 acc_bwp = 0b010 so only the first filter stage is used
if (BMI160_WriteReg(bus, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz) != 0){ if (BMI160_WriteReg(bus, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz) != 0) {
return -3; return -3;
} }
delay(1); delay(1);
// Set gyr_bwp = 0b010 so only the first filter stage is used // Set gyr_bwp = 0b010 so only the first filter stage is used
if (BMI160_WriteReg(bus, BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz) != 0){ if (BMI160_WriteReg(bus, BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz) != 0) {
return -4; return -4;
} }
delay(1); delay(1);
if (BMI160_WriteReg(bus, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G) != 0){ if (BMI160_WriteReg(bus, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G) != 0) {
return -5; return -5;
} }
delay(1); delay(1);
if (BMI160_WriteReg(bus, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS) != 0){ if (BMI160_WriteReg(bus, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS) != 0) {
return -6; return -6;
} }
delay(1); delay(1);
// Enable offset compensation // Enable offset compensation
uint8_t val = spiReadRegister(bus, BMI160_REG_OFFSET_0); uint8_t val = spiReadRegister(bus, BMI160_REG_OFFSET_0);
if (BMI160_WriteReg(bus, BMI160_REG_OFFSET_0, val | 0xC0) != 0){ if (BMI160_WriteReg(bus, BMI160_REG_OFFSET_0, val | 0xC0) != 0) {
return -7; return -7;
} }
// Enable data ready interrupt // Enable data ready interrupt
if (BMI160_WriteReg(bus, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY) != 0){ if (BMI160_WriteReg(bus, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY) != 0) {
return -8; return -8;
} }
delay(1); delay(1);
// Enable INT1 pin // Enable INT1 pin
if (BMI160_WriteReg(bus, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG) != 0){ if (BMI160_WriteReg(bus, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG) != 0) {
return -9; return -9;
} }
delay(1); delay(1);
// Map data ready interrupt to INT1 pin // Map data ready interrupt to INT1 pin
if (BMI160_WriteReg(bus, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY) != 0){ if (BMI160_WriteReg(bus, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY) != 0) {
return -10; return -10;
} }
delay(1); delay(1);

View file

@ -200,7 +200,7 @@ void adcInit(const adcConfig_t *config)
//HAL_CLEANINVALIDATECACHE((uint32_t*)&adcValues, configuredAdcChannels); //HAL_CLEANINVALIDATECACHE((uint32_t*)&adcValues, configuredAdcChannels);
/*##-4- Start the conversion process #######################################*/ /*##-4- Start the conversion process #######################################*/
if(HAL_ADC_Start_DMA(&adc.ADCHandle, (uint32_t*)&adcValues, configuredAdcChannels) != HAL_OK) if (HAL_ADC_Start_DMA(&adc.ADCHandle, (uint32_t*)&adcValues, configuredAdcChannels) != HAL_OK)
{ {
/* Start Conversation Error */ /* Start Conversation Error */
} }

View file

@ -65,7 +65,7 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
}, },
#endif #endif
#ifdef USE_I2C_DEVICE_3 #ifdef USE_I2C_DEVICE_3
{ {
.device = I2CDEV_3, .device = I2CDEV_3,
.reg = I2C3, .reg = I2C3,
.sclPins = { DEFIO_TAG_E(PA8) }, .sclPins = { DEFIO_TAG_E(PA8) },
@ -163,12 +163,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(pHandle ,addr_ << 1, data, len_, I2C_DEFAULT_TIMEOUT); status = HAL_I2C_Master_Transmit(pHandle ,addr_ << 1, data, len_, I2C_DEFAULT_TIMEOUT);
else else
status = HAL_I2C_Mem_Write(pHandle ,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,data, len_, I2C_DEFAULT_TIMEOUT); status = HAL_I2C_Mem_Write(pHandle ,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;
@ -193,12 +193,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(pHandle ,addr_ << 1, buf, len, I2C_DEFAULT_TIMEOUT); status = HAL_I2C_Master_Receive(pHandle ,addr_ << 1, buf, len, I2C_DEFAULT_TIMEOUT);
else else
status = HAL_I2C_Mem_Read(pHandle, addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_DEFAULT_TIMEOUT); status = HAL_I2C_Mem_Read(pHandle, 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

@ -144,7 +144,7 @@ void spiInitDevice(SPIDevice device)
spi->leadingEdge = true; spi->leadingEdge = true;
} }
#endif #endif
// Enable SPI clock // Enable SPI clock
RCC_ClockCmd(spi->rcc, ENABLE); RCC_ClockCmd(spi->rcc, ENABLE);
RCC_ResetCmd(spi->rcc, ENABLE); RCC_ResetCmd(spi->rcc, ENABLE);
@ -154,7 +154,7 @@ void spiInitDevice(SPIDevice device)
IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device)); IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device));
#if defined(STM32F7) #if defined(STM32F7)
if(spi->leadingEdge == true) if (spi->leadingEdge == true)
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF); IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF);
else else
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF); IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF);
@ -162,7 +162,7 @@ void spiInitDevice(SPIDevice device)
IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->mosiAF); IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->mosiAF);
#endif #endif
#if defined(STM32F3) || defined(STM32F4) #if defined(STM32F3) || defined(STM32F4)
if(spi->leadingEdge == true) if (spi->leadingEdge == 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 @@ uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance)
bool spiIsBusBusy(SPI_TypeDef *instance) bool spiIsBusBusy(SPI_TypeDef *instance)
{ {
SPIDevice device = spiDeviceByInstance(instance); SPIDevice device = spiDeviceByInstance(instance);
if(spiDevice[device].hspi.State == HAL_SPI_STATE_BUSY) if (spiDevice[device].hspi.State == HAL_SPI_STATE_BUSY)
return true; return true;
else else
return false; return false;
@ -266,11 +266,11 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len
SPIDevice device = spiDeviceByInstance(instance); SPIDevice device = spiDeviceByInstance(instance);
HAL_StatusTypeDef status; HAL_StatusTypeDef status;
if(!out) // Tx only if (!out) // Tx only
{ {
status = HAL_SPI_Transmit(&spiDevice[device].hspi, (uint8_t *)in, len, SPI_DEFAULT_TIMEOUT); status = HAL_SPI_Transmit(&spiDevice[device].hspi, (uint8_t *)in, len, SPI_DEFAULT_TIMEOUT);
} }
else if(!in) // Rx only else if (!in) // Rx only
{ {
status = HAL_SPI_Receive(&spiDevice[device].hspi, out, len, SPI_DEFAULT_TIMEOUT); status = HAL_SPI_Receive(&spiDevice[device].hspi, out, len, SPI_DEFAULT_TIMEOUT);
} }
@ -279,7 +279,7 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len
status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, in, out, len, SPI_DEFAULT_TIMEOUT); status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, 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

@ -196,7 +196,7 @@ void i2c_OLED_clear_display(busDevice_t *bus)
i2c_OLED_send_cmd(bus, 0x40); // Display start line register to 0 i2c_OLED_send_cmd(bus, 0x40); // Display start line register to 0
i2c_OLED_send_cmd(bus, 0); // Set low col address to 0 i2c_OLED_send_cmd(bus, 0); // Set low col address to 0
i2c_OLED_send_cmd(bus, 0x10); // Set high col address to 0 i2c_OLED_send_cmd(bus, 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(bus, 0x00); // clear i2c_OLED_send_byte(bus, 0x00); // clear
} }
i2c_OLED_send_cmd(bus, 0x81); // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction i2c_OLED_send_cmd(bus, 0x81); // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction
@ -210,7 +210,7 @@ void i2c_OLED_clear_display_quick(busDevice_t *bus)
i2c_OLED_send_cmd(bus, 0x40); // Display start line register to 0 i2c_OLED_send_cmd(bus, 0x40); // Display start line register to 0
i2c_OLED_send_cmd(bus, 0); // Set low col address to 0 i2c_OLED_send_cmd(bus, 0); // Set low col address to 0
i2c_OLED_send_cmd(bus, 0x10); // Set high col address to 0 i2c_OLED_send_cmd(bus, 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(bus, 0x00); // clear i2c_OLED_send_byte(bus, 0x00); // clear
} }
} }

View file

@ -77,7 +77,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

@ -77,7 +77,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(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex) void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex)

View file

@ -78,7 +78,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];
@ -96,7 +96,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]);
@ -108,7 +108,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;
extiChannelRec_t *rec = &extiChannelRecs[chIdx]; extiChannelRec_t *rec = &extiChannelRecs[chIdx];
int group = extiGroups[chIdx]; int group = extiGroups[chIdx];
@ -134,7 +134,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;
@ -154,7 +154,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;
extiChannelRec_t *rec = &extiChannelRecs[chIdx]; extiChannelRec_t *rec = &extiChannelRecs[chIdx];
rec->handler = NULL; rec->handler = NULL;
@ -164,18 +164,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);
@ -188,7 +188,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

@ -40,7 +40,7 @@ static bool timerNChannel = false;
void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim)
{ {
if(htim->Instance == TimHandle.Instance) { if (htim->Instance == TimHandle.Instance) {
//HAL_TIM_PWM_Stop_DMA(&TimHandle,WS2811_TIMER_CHANNEL); //HAL_TIM_PWM_Stop_DMA(&TimHandle,WS2811_TIMER_CHANNEL);
ws2811LedDataTransferInProgress = 0; ws2811LedDataTransferInProgress = 0;
} }
@ -77,7 +77,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag)
TimHandle.Init.Period = period; // 800kHz TimHandle.Init.Period = period; // 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;
} }
@ -116,7 +116,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag)
dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, dmaSource); dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, dmaSource);
/* Initialize TIMx DMA handle */ /* Initialize TIMx DMA handle */
if(HAL_DMA_Init(TimHandle.hdma[dmaSource]) != HAL_OK) { if (HAL_DMA_Init(TimHandle.hdma[dmaSource]) != HAL_OK) {
/* Initialization Error */ /* Initialization Error */
return; return;
} }
@ -139,7 +139,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag)
} }
TIM_OCInitStructure.Pulse = 0; TIM_OCInitStructure.Pulse = 0;
TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
if(HAL_TIM_PWM_ConfigChannel(&TimHandle, &TIM_OCInitStructure, timerChannel) != HAL_OK) { if (HAL_TIM_PWM_ConfigChannel(&TimHandle, &TIM_OCInitStructure, timerChannel) != HAL_OK) {
/* Configuration Error */ /* Configuration Error */
return; return;
} }
@ -154,8 +154,8 @@ void ws2811LedStripDMAEnable(void)
return; return;
} }
if(timerNChannel) { if (timerNChannel) {
if(HAL_TIMEx_PWMN_Start_DMA(&TimHandle, timerChannel, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) { if (HAL_TIMEx_PWMN_Start_DMA(&TimHandle, timerChannel, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) {
/* Starting PWM generation Error */ /* Starting PWM generation Error */
ws2811LedDataTransferInProgress = 0; ws2811LedDataTransferInProgress = 0;
return; return;

View file

@ -120,7 +120,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag)
dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0); dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0);
dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0);
dmaRef = timerHardware->dmaRef; dmaRef = timerHardware->dmaRef;
DMA_DeInit(dmaRef); DMA_DeInit(dmaRef);

View file

@ -330,7 +330,7 @@ void max7456ReInit(void)
ENABLE_MAX7456; ENABLE_MAX7456;
switch(videoSignalCfg) { switch (videoSignalCfg) {
case VIDEO_SYSTEM_PAL: case VIDEO_SYSTEM_PAL:
videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE; videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE;
break; break;
@ -363,7 +363,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);
} }
@ -627,7 +627,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

@ -52,7 +52,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;
@ -100,7 +100,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
{ {
#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; if (Handle == NULL) return;
#endif #endif
configTimeBase(timerHardware->tim, period, hz); configTimeBase(timerHardware->tim, period, hz);
@ -111,7 +111,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
); );
#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);
@ -171,7 +171,7 @@ void pwmWriteMotor(uint8_t index, float value)
{ {
if (pwmMotorsEnabled) { if (pwmMotorsEnabled) {
pwmWrite(index, value); pwmWrite(index, value);
} }
} }
void pwmShutdownPulsesForAllMotors(uint8_t motorCount) void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
@ -203,7 +203,7 @@ bool pwmAreMotorsEnabled(void)
static void pwmCompleteWriteUnused(uint8_t motorCount) static void pwmCompleteWriteUnused(uint8_t motorCount)
{ {
UNUSED(motorCount); UNUSED(motorCount);
} }
static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
@ -226,7 +226,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount)
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
{ {
memset(motors, 0, sizeof(motors)); memset(motors, 0, sizeof(motors));
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm; bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
float sMin = 0; float sMin = 0;
@ -328,7 +328,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
*/ */
motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f; motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000); motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
pwmOutConfig(&motors[motorIndex], timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion); pwmOutConfig(&motors[motorIndex], timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
bool timerAlreadyUsed = false; bool timerAlreadyUsed = false;
@ -466,9 +466,9 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
#ifdef BEEPER #ifdef BEEPER
void pwmWriteBeeper(bool onoffBeep) void pwmWriteBeeper(bool onoffBeep)
{ {
if(!beeperPwm.io) if (!beeperPwm.io)
return; return;
if(onoffBeep == true) { if (onoffBeep == true) {
*beeperPwm.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2; *beeperPwm.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
beeperPwm.enabled = true; beeperPwm.enabled = true;
} else { } else {

View file

@ -169,7 +169,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
DMA_Cmd(dmaRef, DISABLE); DMA_Cmd(dmaRef, DISABLE);
DMA_DeInit(dmaRef); DMA_DeInit(dmaRef);
DMA_StructInit(&DMA_InitStructure); DMA_StructInit(&DMA_InitStructure);
#if defined(STM32F3) #if defined(STM32F3)
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)motor->dmaBuffer; DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)motor->dmaBuffer;

View file

@ -322,7 +322,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

@ -126,11 +126,11 @@ static void TIM_DeInit(TIM_TypeDef *tim)
void setTxSignalEsc(escSerial_t *escSerial, uint8_t state) void setTxSignalEsc(escSerial_t *escSerial, uint8_t state)
{ {
if(escSerial->mode == PROTOCOL_KISSALL) if (escSerial->mode == PROTOCOL_KISSALL)
{ {
for (volatile uint8_t i = 0; i < escSerial->outputCount; i++) { for (volatile uint8_t i = 0; i < escSerial->outputCount; i++) {
uint8_t state_temp = state; uint8_t state_temp = state;
if(escOutputs[i].inverted) { if (escOutputs[i].inverted) {
state_temp ^= ENABLE; state_temp ^= ENABLE;
} }
@ -143,7 +143,7 @@ void setTxSignalEsc(escSerial_t *escSerial, uint8_t state)
} }
else else
{ {
if(escSerial->rxTimerHardware->output & TIMER_OUTPUT_INVERTED) { if (escSerial->rxTimerHardware->output & TIMER_OUTPUT_INVERTED) {
state ^= ENABLE; state ^= ENABLE;
} }
@ -262,7 +262,7 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac
{ {
escSerial_t *escSerial = &(escSerialPorts[portIndex]); escSerial_t *escSerial = &(escSerialPorts[portIndex]);
if(mode != PROTOCOL_KISSALL){ if (mode != PROTOCOL_KISSALL) {
escSerial->rxTimerHardware = &(timerHardware[output]); escSerial->rxTimerHardware = &(timerHardware[output]);
#ifdef USE_HAL_DRIVER #ifdef USE_HAL_DRIVER
escSerial->rxTimerHandle = timerFindTimerHandle(escSerial->rxTimerHardware->tim); escSerial->rxTimerHandle = timerFindTimerHandle(escSerial->rxTimerHardware->tim);
@ -295,7 +295,7 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac
escSerial->escSerialPortIndex = portIndex; escSerial->escSerialPortIndex = portIndex;
if(mode != PROTOCOL_KISSALL) if (mode != PROTOCOL_KISSALL)
{ {
escSerial->txIO = IOGetByTag(escSerial->rxTimerHardware->tag); escSerial->txIO = IOGetByTag(escSerial->rxTimerHardware->tag);
escSerialInputPortConfig(escSerial->rxTimerHardware); escSerialInputPortConfig(escSerial->rxTimerHardware);
@ -303,19 +303,19 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac
} }
delay(50); delay(50);
if(mode==PROTOCOL_SIMONK){ if (mode==PROTOCOL_SIMONK) {
escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex); escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex);
escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex); escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex);
} }
else if(mode==PROTOCOL_BLHELI){ else if (mode==PROTOCOL_BLHELI) {
serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options); serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
} }
else if(mode==PROTOCOL_KISS) { else if (mode==PROTOCOL_KISS) {
escSerialOutputPortConfig(escSerial->rxTimerHardware); // rx is the pin used escSerialOutputPortConfig(escSerial->rxTimerHardware); // rx is the pin used
serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
} }
else if(mode==PROTOCOL_KISSALL) { else if (mode==PROTOCOL_KISSALL) {
escSerial->outputCount = 0; escSerial->outputCount = 0;
memset(&escOutputs, 0, sizeof(escOutputs)); memset(&escOutputs, 0, sizeof(escOutputs));
pwmOutputPort_t *pwmMotors = pwmGetMotors(); pwmOutputPort_t *pwmMotors = pwmGetMotors();
@ -323,10 +323,10 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac
if (pwmMotors[i].enabled) { if (pwmMotors[i].enabled) {
if (pwmMotors[i].io != IO_NONE) { if (pwmMotors[i].io != IO_NONE) {
for (volatile uint8_t j = 0; j < USABLE_TIMER_CHANNEL_COUNT; j++) { for (volatile uint8_t j = 0; j < USABLE_TIMER_CHANNEL_COUNT; j++) {
if(pwmMotors[i].io == IOGetByTag(timerHardware[j].tag)) if (pwmMotors[i].io == IOGetByTag(timerHardware[j].tag))
{ {
escSerialOutputPortConfig(&timerHardware[j]); escSerialOutputPortConfig(&timerHardware[j]);
if(timerHardware[j].output & TIMER_OUTPUT_INVERTED) { if (timerHardware[j].output & TIMER_OUTPUT_INVERTED) {
escOutputs[escSerial->outputCount].inverted = 1; escOutputs[escSerial->outputCount].inverted = 1;
} }
break; break;
@ -340,7 +340,7 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac
setTxSignalEsc(escSerial, ENABLE); setTxSignalEsc(escSerial, ENABLE);
serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
} }
else if(mode == PROTOCOL_CASTLE){ else if (mode == PROTOCOL_CASTLE){
escSerialOutputPortConfig(escSerial->rxTimerHardware); escSerialOutputPortConfig(escSerial->rxTimerHardware);
serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options); serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
@ -361,7 +361,7 @@ void closeEscSerial(escSerialPortIndex_e portIndex, uint8_t mode)
{ {
escSerial_t *escSerial = &(escSerialPorts[portIndex]); escSerial_t *escSerial = &(escSerialPorts[portIndex]);
if(mode != PROTOCOL_KISSALL){ if (mode != PROTOCOL_KISSALL) {
escSerialInputPortDeConfig(escSerial->rxTimerHardware); escSerialInputPortDeConfig(escSerial->rxTimerHardware);
timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL); timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL);
TIM_DeInit(escSerial->rxTimerHardware->tim); TIM_DeInit(escSerial->rxTimerHardware->tim);
@ -381,7 +381,7 @@ void processTxStateEsc(escSerial_t *escSerial)
return; return;
} }
if(transmitStart==0) if (transmitStart==0)
{ {
setTxSignalEsc(escSerial, 1); setTxSignalEsc(escSerial, 1);
} }
@ -394,13 +394,13 @@ reload:
return; return;
} }
if(transmitStart<3) if (transmitStart<3)
{ {
if(transmitStart==0) if (transmitStart==0)
byteToSend = 0xff; byteToSend = 0xff;
if(transmitStart==1) if (transmitStart==1)
byteToSend = 0xff; byteToSend = 0xff;
if(transmitStart==2) if (transmitStart==2)
byteToSend = 0x7f; byteToSend = 0x7f;
transmitStart++; transmitStart++;
} }
@ -425,35 +425,35 @@ reload:
if (escSerial->bitsLeftToTransmit) { if (escSerial->bitsLeftToTransmit) {
mask = escSerial->internalTxBuffer & 1; mask = escSerial->internalTxBuffer & 1;
if(mask) if (mask)
{ {
if(bitq==0 || bitq==1) if (bitq==0 || bitq==1)
{ {
setTxSignalEsc(escSerial, 1); setTxSignalEsc(escSerial, 1);
} }
if(bitq==2 || bitq==3) if (bitq==2 || bitq==3)
{ {
setTxSignalEsc(escSerial, 0); setTxSignalEsc(escSerial, 0);
} }
} }
else else
{ {
if(bitq==0 || bitq==2) if (bitq==0 || bitq==2)
{ {
setTxSignalEsc(escSerial, 1); setTxSignalEsc(escSerial, 1);
} }
if(bitq==1 ||bitq==3) if (bitq==1 ||bitq==3)
{ {
setTxSignalEsc(escSerial, 0); setTxSignalEsc(escSerial, 0);
} }
} }
bitq++; bitq++;
if(bitq>3) if (bitq>3)
{ {
escSerial->internalTxBuffer >>= 1; escSerial->internalTxBuffer >>= 1;
escSerial->bitsLeftToTransmit--; escSerial->bitsLeftToTransmit--;
bitq=0; bitq=0;
if(escSerial->bitsLeftToTransmit==0) if (escSerial->bitsLeftToTransmit==0)
{ {
goto reload; goto reload;
} }
@ -497,7 +497,7 @@ void processTxStateBL(escSerial_t *escSerial)
//set output //set output
if(escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE) { if (escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE) {
escSerialOutputPortConfig(escSerial->rxTimerHardware); escSerialOutputPortConfig(escSerial->rxTimerHardware);
} }
return; return;
@ -514,7 +514,7 @@ void processTxStateBL(escSerial_t *escSerial)
escSerial->isTransmittingData = false; escSerial->isTransmittingData = false;
if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
if(escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE) if (escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE)
{ {
escSerialInputPortConfig(escSerial->rxTimerHardware); escSerialInputPortConfig(escSerial->rxTimerHardware);
} }
@ -683,10 +683,10 @@ void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
UNUSED(capture); UNUSED(capture);
escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb); escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb);
if(escSerial->isReceivingData) if (escSerial->isReceivingData)
{ {
escSerial->receiveTimeout++; escSerial->receiveTimeout++;
if(escSerial->receiveTimeout>8) if (escSerial->receiveTimeout>8)
{ {
escSerial->isReceivingData=0; escSerial->isReceivingData=0;
escSerial->receiveTimeout=0; escSerial->receiveTimeout=0;
@ -714,17 +714,17 @@ void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture
TIM_SetCounter(escSerial->rxTimerHardware->tim,0); TIM_SetCounter(escSerial->rxTimerHardware->tim,0);
#endif #endif
if(capture > 40 && capture < 90) if (capture > 40 && capture < 90)
{ {
zerofirst++; zerofirst++;
if(zerofirst>1) if (zerofirst>1)
{ {
zerofirst=0; zerofirst=0;
escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1; escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1;
bits++; bits++;
} }
} }
else if(capture>90 && capture < 200) else if (capture>90 && capture < 200)
{ {
zerofirst=0; zerofirst=0;
escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1; escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1;
@ -733,7 +733,7 @@ void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture
} }
else else
{ {
if(!escSerial->isReceivingData) if (!escSerial->isReceivingData)
{ {
//start //start
//lets reset //lets reset
@ -749,11 +749,11 @@ void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture
} }
escSerial->receiveTimeout = 0; escSerial->receiveTimeout = 0;
if(bits==8) if (bits==8)
{ {
bits=0; bits=0;
bytes++; bytes++;
if(bytes>3) if (bytes>3)
{ {
extractAndStoreRxByteEsc(escSerial); extractAndStoreRxByteEsc(escSerial);
} }
@ -852,7 +852,7 @@ void escSerialInitialize()
for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
// set outputs to pullup // set outputs to pullup
if(timerHardware[i].output & TIMER_OUTPUT_ENABLED) if (timerHardware[i].output & TIMER_OUTPUT_ENABLED)
{ {
escSerialGPIOConfig(&timerHardware[i], IOCFG_IPU); //GPIO_Mode_IPU escSerialGPIOConfig(&timerHardware[i], IOCFG_IPU); //GPIO_Mode_IPU
} }
@ -916,7 +916,7 @@ static bool ProcessExitCommand(uint8_t c)
if (currentPort.checksum == c) { if (currentPort.checksum == c) {
currentPort.c_state = COMMAND_RECEIVED; currentPort.c_state = COMMAND_RECEIVED;
if((currentPort.cmdMSP == 0xF4) && (currentPort.dataSize==0)) if ((currentPort.cmdMSP == 0xF4) && (currentPort.dataSize==0))
{ {
currentPort.c_state = IDLE; currentPort.c_state = IDLE;
return true; return true;
@ -953,14 +953,14 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin
break; break;
} }
if((mode == PROTOCOL_KISS) && (output == 255)){ if ((mode == PROTOCOL_KISS) && (output == 255)) {
motor_output = 255; motor_output = 255;
mode = PROTOCOL_KISSALL; mode = PROTOCOL_KISSALL;
} }
else { else {
uint8_t first_output = 0; uint8_t first_output = 0;
for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
if(timerHardware[i].output & TIMER_OUTPUT_ENABLED) if (timerHardware[i].output & TIMER_OUTPUT_ENABLED)
{ {
first_output=i; first_output=i;
break; break;
@ -969,18 +969,18 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin
//doesn't work with messy timertable //doesn't work with messy timertable
motor_output=first_output+output-1; motor_output=first_output+output-1;
if(motor_output >=USABLE_TIMER_CHANNEL_COUNT) if (motor_output >=USABLE_TIMER_CHANNEL_COUNT)
return; return;
} }
escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, escBaudrate, 0, mode); escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, escBaudrate, 0, mode);
uint8_t ch; uint8_t ch;
while(1) { while (1) {
if(mode!=2) if (mode!=2)
{ {
if (serialRxBytesWaiting(escPort)) { if (serialRxBytesWaiting(escPort)) {
LED0_ON; LED0_ON;
while(serialRxBytesWaiting(escPort)) while (serialRxBytesWaiting(escPort))
{ {
ch = serialRead(escPort); ch = serialRead(escPort);
serialWrite(escPassthroughPort, ch); serialWrite(escPassthroughPort, ch);
@ -990,11 +990,11 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin
} }
if (serialRxBytesWaiting(escPassthroughPort)) { if (serialRxBytesWaiting(escPassthroughPort)) {
LED1_ON; LED1_ON;
while(serialRxBytesWaiting(escPassthroughPort)) while (serialRxBytesWaiting(escPassthroughPort))
{ {
ch = serialRead(escPassthroughPort); ch = serialRead(escPassthroughPort);
exitEsc = ProcessExitCommand(ch); exitEsc = ProcessExitCommand(ch);
if(exitEsc) if (exitEsc)
{ {
serialWrite(escPassthroughPort, 0x24); serialWrite(escPassthroughPort, 0x24);
serialWrite(escPassthroughPort, 0x4D); serialWrite(escPassthroughPort, 0x4D);
@ -1005,14 +1005,14 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin
closeEscSerial(ESCSERIAL1, mode); closeEscSerial(ESCSERIAL1, mode);
return; return;
} }
if(mode==PROTOCOL_BLHELI){ if (mode==PROTOCOL_BLHELI) {
serialWrite(escPassthroughPort, ch); // blheli loopback serialWrite(escPassthroughPort, ch); // blheli loopback
} }
serialWrite(escPort, ch); serialWrite(escPort, ch);
} }
LED1_OFF; LED1_OFF;
} }
if(mode != PROTOCOL_CASTLE){ if (mode != PROTOCOL_CASTLE) {
delay(5); delay(5);
} }
} }

View file

@ -53,7 +53,7 @@ static void onClose(dyad_Event *e) {
s->clientCount--; s->clientCount--;
s->conn = NULL; s->conn = NULL;
fprintf(stderr, "[CLS]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount); fprintf(stderr, "[CLS]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount);
if(s->clientCount == 0) { if (s->clientCount == 0) {
s->connected = false; s->connected = false;
} }
} }
@ -62,7 +62,7 @@ static void onAccept(dyad_Event *e) {
fprintf(stderr, "New connection on UART%u, %d\n", s->id + 1, s->clientCount); fprintf(stderr, "New connection on UART%u, %d\n", s->id + 1, s->clientCount);
s->connected = true; s->connected = true;
if(s->clientCount > 0) { if (s->clientCount > 0) {
dyad_close(e->remote); dyad_close(e->remote);
return; return;
} }
@ -76,7 +76,7 @@ static void onAccept(dyad_Event *e) {
} }
static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id) static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id)
{ {
if(tcpPortInitialized[id]) { if (tcpPortInitialized[id]) {
fprintf(stderr, "port is already initialized!\n"); fprintf(stderr, "port is already initialized!\n");
return s; return s;
} }
@ -103,7 +103,7 @@ static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id)
dyad_setNoDelay(s->serv, 1); dyad_setNoDelay(s->serv, 1);
dyad_addListener(s->serv, DYAD_EVENT_ACCEPT, onAccept, s); dyad_addListener(s->serv, DYAD_EVENT_ACCEPT, onAccept, s);
if(dyad_listenEx(s->serv, NULL, BASE_PORT + id + 1, 10) == 0) { if (dyad_listenEx(s->serv, NULL, BASE_PORT + id + 1, 10) == 0) {
fprintf(stderr, "bind port %u for UART%u\n", (unsigned)BASE_PORT + id + 1, (unsigned)id + 1); fprintf(stderr, "bind port %u for UART%u\n", (unsigned)BASE_PORT + id + 1, (unsigned)id + 1);
} else { } else {
fprintf(stderr, "bind port %u for UART%u failed!!\n", (unsigned)BASE_PORT + id + 1, (unsigned)id + 1); fprintf(stderr, "bind port %u for UART%u failed!!\n", (unsigned)BASE_PORT + id + 1, (unsigned)id + 1);
@ -116,11 +116,11 @@ serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, uint32_t b
tcpPort_t *s = NULL; tcpPort_t *s = NULL;
#if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8) #if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8)
if(id >= 0 && id < SERIAL_PORT_COUNT) { if (id >= 0 && id < SERIAL_PORT_COUNT) {
s = tcpReconfigure(&tcpSerialPorts[id], id); s = tcpReconfigure(&tcpSerialPorts[id], id);
} }
#endif #endif
if(!s) if (!s)
return NULL; return NULL;
s->port.vTable = &tcpVTable; s->port.vTable = &tcpVTable;
@ -219,7 +219,7 @@ void tcpWrite(serialPort_t *instance, uint8_t ch)
void tcpDataOut(tcpPort_t *instance) void tcpDataOut(tcpPort_t *instance)
{ {
tcpPort_t *s = (tcpPort_t *)instance; tcpPort_t *s = (tcpPort_t *)instance;
if(s->conn == NULL) return; if (s->conn == NULL) return;
pthread_mutex_lock(&s->txLock); pthread_mutex_lock(&s->txLock);
if (s->port.txBufferHead < s->port.txBufferTail) { if (s->port.txBufferHead < s->port.txBufferTail) {
@ -229,7 +229,7 @@ void tcpDataOut(tcpPort_t *instance)
s->port.txBufferTail = 0; s->port.txBufferTail = 0;
} }
int chunk = s->port.txBufferHead - s->port.txBufferTail; int chunk = s->port.txBufferHead - s->port.txBufferTail;
if(chunk) if (chunk)
dyad_write(s->conn, (const void*)&s->port.txBuffer[s->port.txBufferTail], chunk); dyad_write(s->conn, (const void*)&s->port.txBuffer[s->port.txBufferTail], chunk);
s->port.txBufferTail = s->port.txBufferHead; s->port.txBufferTail = s->port.txBufferHead;
@ -241,7 +241,7 @@ void tcpDataIn(tcpPort_t *instance, uint8_t* ch, int size)
tcpPort_t *s = (tcpPort_t *)instance; tcpPort_t *s = (tcpPort_t *)instance;
pthread_mutex_lock(&s->rxLock); pthread_mutex_lock(&s->rxLock);
while(size--) { while (size--) {
// printf("%c", *ch); // printf("%c", *ch);
s->port.rxBuffer[s->port.rxBufferHead] = *(ch++); s->port.rxBuffer[s->port.rxBufferHead] = *(ch++);
if (s->port.rxBufferHead + 1 >= s->port.rxBufferSize) { if (s->port.rxBufferHead + 1 >= s->port.rxBufferSize) {

View file

@ -43,7 +43,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)
{ {
@ -94,7 +94,7 @@ 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);
} }
@ -167,9 +167,9 @@ 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);
@ -225,7 +225,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

@ -104,7 +104,7 @@ 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

@ -74,7 +74,7 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = {
}, },
#endif #endif
#ifdef USE_UART2 #ifdef USE_UART2
{ {
.device = UARTDEV_2, .device = UARTDEV_2,
.reg = USART2, .reg = USART2,
.rxDMAChannel = UART2_RX_DMA_CHANNEL, .rxDMAChannel = UART2_RX_DMA_CHANNEL,
@ -101,8 +101,8 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = {
.irqn = USART3_IRQn, .irqn = USART3_IRQn,
.txPriority = NVIC_PRIO_SERIALUART3, .txPriority = NVIC_PRIO_SERIALUART3,
.rxPriority = NVIC_PRIO_SERIALUART3 .rxPriority = NVIC_PRIO_SERIALUART3
}, },
#endif #endif
}; };
void uart_tx_dma_IRQHandler(dmaChannelDescriptor_t* descriptor) void uart_tx_dma_IRQHandler(dmaChannelDescriptor_t* descriptor)

View file

@ -228,7 +228,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);
@ -247,37 +247,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);
@ -308,7 +308,7 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
uartDevice_t *uartdev = uartDevmap[device]; uartDevice_t *uartdev = uartDevmap[device];
if (!uartdev) { if (!uartdev) {
return NULL; return NULL;
} }
uartPort_t *s = &(uartdev->port); uartPort_t *s = &(uartdev->port);
@ -370,7 +370,7 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
//HAL_NVIC_SetPriority(hardware->txIrq, NVIC_PRIORITY_BASE(hardware->txPriority), NVIC_PRIORITY_SUB(hardware->txPriority)); //HAL_NVIC_SetPriority(hardware->txIrq, NVIC_PRIORITY_BASE(hardware->txPriority), NVIC_PRIORITY_SUB(hardware->txPriority));
//HAL_NVIC_EnableIRQ(hardware->txIrq); //HAL_NVIC_EnableIRQ(hardware->txIrq);
if(!s->rxDMAChannel) if (!s->rxDMAChannel)
{ {
HAL_NVIC_SetPriority(hardware->rxIrq, NVIC_PRIORITY_BASE(hardware->rxPriority), NVIC_PRIORITY_SUB(hardware->rxPriority)); HAL_NVIC_SetPriority(hardware->rxIrq, NVIC_PRIORITY_BASE(hardware->rxPriority), NVIC_PRIORITY_SUB(hardware->rxPriority));
HAL_NVIC_EnableIRQ(hardware->rxIrq); HAL_NVIC_EnableIRQ(hardware->rxIrq);

View file

@ -35,7 +35,7 @@ static uint16_t beeperFrequency = 0;
void systemBeep(bool onoff) void systemBeep(bool onoff)
{ {
#ifdef BEEPER #ifdef BEEPER
if(beeperFrequency == 0) { if (beeperFrequency == 0) {
IOWrite(beeperIO, beeperInverted ? onoff : !onoff); IOWrite(beeperIO, beeperInverted ? onoff : !onoff);
} else { } else {
pwmWriteBeeper(onoff); pwmWriteBeeper(onoff);
@ -48,7 +48,7 @@ void systemBeep(bool onoff)
void systemBeepToggle(void) void systemBeepToggle(void)
{ {
#ifdef BEEPER #ifdef BEEPER
if(beeperFrequency == 0) { if (beeperFrequency == 0) {
IOToggle(beeperIO); IOToggle(beeperIO);
} else { } else {
pwmToggleBeeper(); pwmToggleBeeper();
@ -60,7 +60,7 @@ void beeperInit(const beeperDevConfig_t *config)
{ {
#ifdef BEEPER #ifdef BEEPER
beeperFrequency = config->frequency; beeperFrequency = config->frequency;
if(beeperFrequency == 0) { if (beeperFrequency == 0) {
beeperIO = IOGetByTag(config->ioTag); beeperIO = IOGetByTag(config->ioTag);
beeperInverted = config->isInverted; beeperInverted = config->isInverted;
if (beeperIO) { if (beeperIO) {

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
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) {
#if defined(STM32F10X) #if defined(STM32F10X)
case TIM1_CC_IRQn: case TIM1_CC_IRQn:
timerNVICConfigure(TIM1_UP_IRQn); timerNVICConfigure(TIM1_UP_IRQn);
@ -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);
@ -327,8 +327,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;
} }
@ -346,13 +346,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);
@ -371,9 +371,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
@ -383,11 +383,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);
} }
@ -433,8 +433,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;
} }
@ -504,7 +504,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) {
@ -541,17 +541,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 {
@ -559,7 +559,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;
} }
@ -584,7 +584,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;
} }
@ -714,10 +714,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;
} }
} }
@ -728,18 +728,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, uint32_t hz)
if (timerIndex >= USED_TIMER_COUNT) { if (timerIndex >= USED_TIMER_COUNT) {
return; return;
} }
if(timerHandle[timerIndex].Handle.Instance == tim) if (timerHandle[timerIndex].Handle.Instance == tim)
{ {
// already configured // already configured
return; return;
@ -263,7 +263,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz)
timerHandle[timerIndex].Handle.Init.RepetitionCounter = 0x0000; timerHandle[timerIndex].Handle.Init.RepetitionCounter = 0x0000;
HAL_TIM_Base_Init(&timerHandle[timerIndex].Handle); HAL_TIM_Base_Init(&timerHandle[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;
memset(&sClockSourceConfig, 0, sizeof(sClockSourceConfig)); memset(&sClockSourceConfig, 0, sizeof(sClockSourceConfig));
@ -273,7 +273,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz)
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;
memset(&sMasterConfig, 0, sizeof(sMasterConfig)); memset(&sMasterConfig, 0, sizeof(sMasterConfig));
@ -300,7 +300,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);
@ -320,14 +320,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(&timerHandle[timerIndex].Handle); HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle);
@ -361,15 +361,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(&timerHandle[timerIndex].Handle, TIM_IT_UPDATE); __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_UPDATE);
else else
__HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_UPDATE); __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_UPDATE);
@ -383,13 +383,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(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); __HAL_TIM_DISABLE_IT(&timerHandle[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(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim); timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim);
@ -408,9 +408,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(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo)); __HAL_TIM_DISABLE_IT(&timerHandle[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(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi)); __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
// setup callback info // setup callback info
@ -420,11 +420,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(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo)); __HAL_TIM_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
__HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo)); __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
} }
if(edgeCallbackHi) { if (edgeCallbackHi) {
__HAL_TIM_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi)); __HAL_TIM_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
__HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi)); __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
} }
@ -443,7 +443,7 @@ void timerChITConfigDualLo(const timerHardware_t *timHw, FunctionalState newStat
return; return;
} }
if(newState) if (newState)
__HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2)); __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2));
else else
__HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2)); __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2));
@ -462,7 +462,7 @@ void timerChITConfig(const timerHardware_t *timHw, FunctionalState newState)
return; return;
} }
if(newState) if (newState)
__HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
else else
__HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
@ -505,8 +505,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;
} }
@ -515,7 +515,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;
@ -532,7 +532,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;
@ -579,7 +579,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;
@ -593,7 +593,7 @@ void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHig
HAL_TIM_OC_ConfigChannel(&timerHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel); HAL_TIM_OC_ConfigChannel(&timerHandle[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(&timerHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel); HAL_TIM_OC_ConfigChannel(&timerHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel);
HAL_TIM_OC_Start(&timerHandle[timer].Handle, timHw->channel); HAL_TIM_OC_Start(&timerHandle[timer].Handle, timHw->channel);
@ -610,17 +610,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 {
@ -628,7 +628,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;
} }
@ -653,7 +653,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;
} }
@ -812,10 +812,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;
} }
} }
@ -826,18 +826,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;
@ -913,4 +913,4 @@ uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz)
return 0; return 0;
} }
return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1; return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1;
} }

View file

@ -188,7 +188,7 @@ void transponderIrWaitForTransmitComplete(void)
{ {
static uint32_t waitCounter = 0; static uint32_t waitCounter = 0;
while(transponderIrDataTransferInProgress) { while (transponderIrDataTransferInProgress) {
waitCounter++; waitCounter++;
} }
} }

View file

@ -55,7 +55,7 @@ void updateTransponderDMABufferERLT(transponder_t *transponder, const uint8_t* t
addBitToBuffer(transponder, ERLTCyclesForZeroBit, ERLTBitQuiet); addBitToBuffer(transponder, ERLTCyclesForZeroBit, ERLTBitQuiet);
//add data bits, only the 6 LSB //add data bits, only the 6 LSB
for(int i = 5; i >= 0; i--) for (int i = 5; i >= 0; i--)
{ {
uint8_t bv = (byteToSend >> i) & 0x01; uint8_t bv = (byteToSend >> i) & 0x01;
paritysum += bv; paritysum += bv;

View file

@ -67,7 +67,7 @@ void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel)
if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount)) if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount))
return; return;
if (vtxDevice->vTable->setBandAndChannel) if (vtxDevice->vTable->setBandAndChannel)
vtxDevice->vTable->setBandAndChannel(band, channel); vtxDevice->vTable->setBandAndChannel(band, channel);
} }
@ -80,7 +80,7 @@ void vtxCommonSetPowerByIndex(uint8_t index)
if (index > vtxDevice->capability.powerCount) if (index > vtxDevice->capability.powerCount)
return; return;
if (vtxDevice->vTable->setPowerByIndex) if (vtxDevice->vTable->setPowerByIndex)
vtxDevice->vTable->setPowerByIndex(index); vtxDevice->vTable->setPowerByIndex(index);
} }

View file

@ -326,7 +326,7 @@ static void printValuePointer(const clivalue_t *var, const void *valuePointer, b
break; break;
} }
switch(var->type & VALUE_MODE_MASK) { switch (var->type & VALUE_MODE_MASK) {
case MODE_DIRECT: case MODE_DIRECT:
cliPrintf("%d", value); cliPrintf("%d", value);
if (full) { if (full) {
@ -805,7 +805,7 @@ static void cliSerial(char *cmdline)
break; break;
} }
switch(i) { switch (i) {
case 0: case 0:
if (baudRateIndex < BAUD_9600 || baudRateIndex > BAUD_1000000) { if (baudRateIndex < BAUD_9600 || baudRateIndex > BAUD_1000000) {
continue; continue;
@ -859,7 +859,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;
@ -1325,7 +1325,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;
} }
@ -2016,7 +2016,7 @@ static void cliBeeper(char *cmdline)
if (len == 0) { if (len == 0) {
cliPrintf("Disabled:"); cliPrintf("Disabled:");
for (int32_t i = 0; ; i++) { for (int32_t i = 0; ; i++) {
if (i == beeperCount - 2){ if (i == beeperCount - 2) {
if (mask == 0) if (mask == 0)
cliPrint(" none"); cliPrint(" none");
break; break;
@ -2126,7 +2126,7 @@ static void cliMap(char *cmdline)
static char *checkCommand(char *cmdLine, const char *command) static char *checkCommand(char *cmdLine, const char *command)
{ {
if(!strncasecmp(cmdLine, command, strlen(command)) // command names match if (!strncasecmp(cmdLine, command, strlen(command)) // command names match
&& (isspace((unsigned)cmdLine[strlen(command)]) || cmdLine[strlen(command)] == 0)) { && (isspace((unsigned)cmdLine[strlen(command)]) || cmdLine[strlen(command)] == 0)) {
return cmdLine + strlen(command) + 1; return cmdLine + strlen(command) + 1;
} else { } else {
@ -2280,13 +2280,13 @@ static void cliEscPassthrough(char *cmdline)
while (pch != NULL) { while (pch != NULL) {
switch (pos) { switch (pos) {
case 0: case 0:
if(strncasecmp(pch, "sk", strlen(pch)) == 0) { if (strncasecmp(pch, "sk", strlen(pch)) == 0) {
mode = PROTOCOL_SIMONK; mode = PROTOCOL_SIMONK;
} else if(strncasecmp(pch, "bl", strlen(pch)) == 0) { } else if (strncasecmp(pch, "bl", strlen(pch)) == 0) {
mode = PROTOCOL_BLHELI; mode = PROTOCOL_BLHELI;
} else if(strncasecmp(pch, "ki", strlen(pch)) == 0) { } else if (strncasecmp(pch, "ki", strlen(pch)) == 0) {
mode = PROTOCOL_KISS; mode = PROTOCOL_KISS;
} else if(strncasecmp(pch, "cc", strlen(pch)) == 0) { } else if (strncasecmp(pch, "cc", strlen(pch)) == 0) {
mode = PROTOCOL_KISSALL; mode = PROTOCOL_KISSALL;
} else { } else {
cliShowParseError(); cliShowParseError();
@ -3464,7 +3464,7 @@ void cliProcess(void)
break; break;
} }
} }
if(cmd < cmdTable + ARRAYLEN(cmdTable)) if (cmd < cmdTable + ARRAYLEN(cmdTable))
cmd->func(options); cmd->func(options);
else else
cliPrint("Unknown command, try 'help'"); cliPrint("Unknown command, try 'help'");

View file

@ -351,7 +351,7 @@ void validateAndFixConfig(void)
#endif #endif
#ifndef USE_OSD_SLAVE #ifndef USE_OSD_SLAVE
if((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){ if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) {
motorConfigMutable()->mincommand = 1000; motorConfigMutable()->mincommand = 1000;
} }
@ -467,7 +467,7 @@ void validateAndFixGyroConfig(void)
// check for looptime restrictions based on motor protocol. Motor times have safety margin // check for looptime restrictions based on motor protocol. Motor times have safety margin
const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom; const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom;
float motorUpdateRestriction; float motorUpdateRestriction;
switch(motorConfig()->dev.motorPwmProtocol) { switch (motorConfig()->dev.motorPwmProtocol) {
case (PWM_TYPE_STANDARD): case (PWM_TYPE_STANDARD):
motorUpdateRestriction = 1.0f/BRUSHLESS_MOTORS_PWM_RATE; motorUpdateRestriction = 1.0f/BRUSHLESS_MOTORS_PWM_RATE;
break; break;
@ -498,7 +498,7 @@ void validateAndFixGyroConfig(void)
if (motorConfig()->dev.useUnsyncedPwm && (motorConfig()->dev.motorPwmProtocol <= PWM_TYPE_BRUSHED) && motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD) { if (motorConfig()->dev.useUnsyncedPwm && (motorConfig()->dev.motorPwmProtocol <= PWM_TYPE_BRUSHED) && motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD) {
uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction); uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
if(motorConfig()->dev.motorPwmRate > maxEscRate) if (motorConfig()->dev.motorPwmRate > maxEscRate)
motorConfigMutable()->dev.motorPwmRate = maxEscRate; motorConfigMutable()->dev.motorPwmRate = maxEscRate;
} }
} }

View file

@ -649,7 +649,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
static uint8_t pidUpdateCountdown; static uint8_t pidUpdateCountdown;
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC) #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
if(lockMainPID() != 0) return; if (lockMainPID() != 0) return;
#endif #endif
if (debugMode == DEBUG_CYCLETIME) { if (debugMode == DEBUG_CYCLETIME) {

View file

@ -42,8 +42,8 @@ void dispatchEnable(void)
void dispatchProcess(uint32_t currentTime) void dispatchProcess(uint32_t currentTime)
{ {
for(dispatchEntry_t **p = &head; *p; ) { for (dispatchEntry_t **p = &head; *p; ) {
if(cmp32(currentTime, (*p)->delayedUntil) < 0) if (cmp32(currentTime, (*p)->delayedUntil) < 0)
break; break;
// unlink entry first, so handler can replan self // unlink entry first, so handler can replan self
dispatchEntry_t *current = *p; dispatchEntry_t *current = *p;
@ -56,7 +56,7 @@ void dispatchAdd(dispatchEntry_t *entry, int delayUs)
{ {
uint32_t delayedUntil = micros() + delayUs; uint32_t delayedUntil = micros() + delayUs;
dispatchEntry_t **p = &head; dispatchEntry_t **p = &head;
while(*p && cmp32((*p)->delayedUntil, delayedUntil) < 0) while (*p && cmp32((*p)->delayedUntil, delayedUntil) < 0)
p = &(*p)->next; p = &(*p)->next;
entry->next = *p; entry->next = *p;
*p = entry; *p = entry;

View file

@ -455,10 +455,10 @@ void init(void)
#endif #endif
#ifdef USE_I2C_DEVICE_3 #ifdef USE_I2C_DEVICE_3
i2cInit(I2CDEV_3); i2cInit(I2CDEV_3);
#endif #endif
#ifdef USE_I2C_DEVICE_4 #ifdef USE_I2C_DEVICE_4
i2cInit(I2CDEV_4); i2cInit(I2CDEV_4);
#endif #endif
#endif /* USE_I2C */ #endif /* USE_I2C */
#endif /* TARGET_BUS_INIT */ #endif /* TARGET_BUS_INIT */

View file

@ -214,7 +214,7 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr
escPortIndex = sbufReadU8(src); escPortIndex = sbufReadU8(src);
} }
switch(escMode) { switch (escMode) {
case ESC_4WAY: case ESC_4WAY:
// get channel number // get channel number
// switch all motor lines HI // switch all motor lines HI
@ -284,7 +284,7 @@ const box_t *findBoxByPermanentId(uint8_t permanentId)
static bool activeBoxIdGet(boxId_e boxId) static bool activeBoxIdGet(boxId_e boxId)
{ {
if(boxId > sizeof(activeBoxIds) * 8) if (boxId > sizeof(activeBoxIds) * 8)
return false; return false;
return bitArrayGet(&activeBoxIds, boxId); return bitArrayGet(&activeBoxIds, boxId);
} }
@ -328,7 +328,7 @@ void initActiveBoxIds(void)
memset(&ena, 0, sizeof(ena)); memset(&ena, 0, sizeof(ena));
// macro to enable boxId (BoxidMaskEnable). Reference to ena is hidden, local use only // macro to enable boxId (BoxidMaskEnable). Reference to ena is hidden, local use only
#define BME(boxId) do { bitArraySet(&ena, boxId); } while(0) #define BME(boxId) do { bitArraySet(&ena, boxId); } while (0)
BME(BOXARM); BME(BOXARM);
if (!feature(FEATURE_AIRMODE)) { if (!feature(FEATURE_AIRMODE)) {

View file

@ -139,7 +139,7 @@ static void scaleRcCommandToFpvCamAngle(void) {
static float cosFactor = 1.0; static float cosFactor = 1.0;
static float sinFactor = 0.0; static float sinFactor = 0.0;
if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees){ if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees) {
lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees; lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD); cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD);
sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD); sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
@ -167,7 +167,7 @@ static void scaleRcCommandToFpvCamAngle(void) {
const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
if(ABS(rcCommandSpeed) > throttleVelocityThreshold) if (ABS(rcCommandSpeed) > throttleVelocityThreshold)
pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain)); pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
else else
pidSetItermAccelerator(1.0f); pidSetItermAccelerator(1.0f);
@ -193,7 +193,7 @@ void processRcCommand(void)
if (rxConfig()->rcInterpolation) { if (rxConfig()->rcInterpolation) {
// Set RC refresh rate for sampling and channels to filter // Set RC refresh rate for sampling and channels to filter
switch(rxConfig()->rcInterpolation) { switch (rxConfig()->rcInterpolation) {
case(RC_SMOOTHING_AUTO): case(RC_SMOOTHING_AUTO):
rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps
break; break;

View file

@ -221,7 +221,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
beeperConfirmationBeeps(delta > 0 ? 2 : 1); beeperConfirmationBeeps(delta > 0 ? 2 : 1);
int newValue; int newValue;
switch(adjustmentFunction) { switch (adjustmentFunction) {
case ADJUSTMENT_RC_RATE: case ADJUSTMENT_RC_RATE:
newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c
controlRateConfig->rcRate8 = newValue; controlRateConfig->rcRate8 = newValue;
@ -339,7 +339,7 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
{ {
uint8_t beeps = 0; uint8_t beeps = 0;
switch(adjustmentFunction) { switch (adjustmentFunction) {
case ADJUSTMENT_RATE_PROFILE: case ADJUSTMENT_RATE_PROFILE:
{ {
if (getCurrentControlRateProfileIndex() != position) { if (getCurrentControlRateProfileIndex() != position) {
@ -352,7 +352,7 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
case ADJUSTMENT_HORIZON_STRENGTH: case ADJUSTMENT_HORIZON_STRENGTH:
{ {
uint8_t newValue = constrain(position, 0, 200); // FIXME magic numbers repeated in serial_cli.c uint8_t newValue = constrain(position, 0, 200); // FIXME magic numbers repeated in serial_cli.c
if(pidProfile->pid[PID_LEVEL].D != newValue) { if (pidProfile->pid[PID_LEVEL].D != newValue) {
beeps = ((newValue - pidProfile->pid[PID_LEVEL].D) / 8) + 1; beeps = ((newValue - pidProfile->pid[PID_LEVEL].D) / 8) + 1;
pidProfile->pid[PID_LEVEL].D = newValue; pidProfile->pid[PID_LEVEL].D = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_HORIZON_STRENGTH, position); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_HORIZON_STRENGTH, position);

View file

@ -317,7 +317,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 > 0.0f) { if (imuRuntimeConfig.dcm_ki > 0.0f) {
// Stop integrating if spinning beyond the certain limit // Stop integrating if spinning beyond the certain limit
if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) { if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
const float dcmKiGain = imuRuntimeConfig.dcm_ki; const float dcmKiGain = imuRuntimeConfig.dcm_ki;
@ -455,7 +455,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) { if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) {
IMU_LOCK; IMU_LOCK;
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC) #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
if(imuUpdated == false){ if (imuUpdated == false) {
IMU_UNLOCK; IMU_UNLOCK;
return; return;
} }

View file

@ -339,7 +339,7 @@ bool mixerIsOutputSaturated(int axis, float errorRate)
// All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
// DSHOT scaling is done to the actual dshot range // DSHOT scaling is done to the actual dshot range
void initEscEndpoints(void) { void initEscEndpoints(void) {
switch(motorConfig()->dev.motorPwmProtocol) { switch (motorConfig()->dev.motorPwmProtocol) {
#ifdef USE_DSHOT #ifdef USE_DSHOT
case PWM_TYPE_PROSHOT1000: case PWM_TYPE_PROSHOT1000:
case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT1200:
@ -516,7 +516,7 @@ void mixTable(pidProfile_t *pidProfile)
throttlePrevious = rcCommand[THROTTLE]; throttlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; throttle = rcCommand[THROTTLE] - rxConfig()->mincheck;
currentThrottleInputRange = rcCommandThrottleRange3dLow; currentThrottleInputRange = rcCommandThrottleRange3dLow;
if(isMotorProtocolDshot()) mixerInversion = true; if (isMotorProtocolDshot()) mixerInversion = true;
} else if (rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { // Positive handling } else if (rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { // Positive handling
motorOutputMax = motorOutputHigh; motorOutputMax = motorOutputHigh;
motorOutputMin = deadbandMotor3dHigh; motorOutputMin = deadbandMotor3dHigh;
@ -528,7 +528,7 @@ void mixTable(pidProfile_t *pidProfile)
motorOutputMin = motorOutputLow; motorOutputMin = motorOutputLow;
throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
currentThrottleInputRange = rcCommandThrottleRange3dLow; currentThrottleInputRange = rcCommandThrottleRange3dLow;
if(isMotorProtocolDshot()) mixerInversion = true; if (isMotorProtocolDshot()) mixerInversion = true;
} else { // Deadband handling from positive to negative } else { // Deadband handling from positive to negative
motorOutputMax = motorOutputHigh; motorOutputMax = motorOutputHigh;
motorOutputMin = deadbandMotor3dHigh; motorOutputMin = deadbandMotor3dHigh;
@ -561,8 +561,8 @@ void mixTable(pidProfile_t *pidProfile)
float motorMixMax = 0, motorMixMin = 0; float motorMixMax = 0, motorMixMin = 0;
const int yawDirection = GET_DIRECTION(mixerConfig()->yaw_motors_reversed); const int yawDirection = GET_DIRECTION(mixerConfig()->yaw_motors_reversed);
int motorDirection = GET_DIRECTION(isMotorsReversed()); int motorDirection = GET_DIRECTION(isMotorsReversed());
for (int i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {
float mix = float mix =
scaledAxisPidRoll * currentMixer[i].roll * (motorDirection) + scaledAxisPidRoll * currentMixer[i].roll * (motorDirection) +

View file

@ -243,7 +243,7 @@ static float crashDtermThreshold;
static float crashGyroThreshold; static float crashGyroThreshold;
void pidInitConfig(const pidProfile_t *pidProfile) { void pidInitConfig(const pidProfile_t *pidProfile) {
for(int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
Kp[axis] = PTERM_SCALE * pidProfile->pid[axis].P; Kp[axis] = PTERM_SCALE * pidProfile->pid[axis].P;
Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I; Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I;
Kd[axis] = DTERM_SCALE * pidProfile->pid[axis].D; Kd[axis] = DTERM_SCALE * pidProfile->pid[axis].D;
@ -359,7 +359,7 @@ static float accelerationLimit(int axis, float currentPidSetpoint) {
static float previousSetpoint[3]; static float previousSetpoint[3];
const float currentVelocity = currentPidSetpoint- previousSetpoint[axis]; const float currentVelocity = currentPidSetpoint- previousSetpoint[axis];
if(ABS(currentVelocity) > maxVelocity[axis]) if (ABS(currentVelocity) > maxVelocity[axis])
currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis]; currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis];
previousSetpoint[axis] = currentPidSetpoint; previousSetpoint[axis] = currentPidSetpoint;
@ -383,7 +383,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
float currentPidSetpoint = getSetpointRate(axis); float currentPidSetpoint = getSetpointRate(axis);
if(maxVelocity[axis]) if (maxVelocity[axis])
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint); currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
// Yaw control is GYRO based, direct sticks control is applied to rate PID // Yaw control is GYRO based, direct sticks control is applied to rate PID

View file

@ -363,7 +363,7 @@ void beeperUpdate(timeUs_t currentTimeUs)
} }
#ifdef USE_DSHOT #ifdef USE_DSHOT
if (!ARMING_FLAG(ARMED) && currentBeeperEntry->mode == BEEPER_RX_SET) { if (!ARMING_FLAG(ARMED) && currentBeeperEntry->mode == BEEPER_RX_SET) {
for (unsigned index = 0; index < getMotorCount(); index++) { for (unsigned index = 0; index < getMotorCount(); index++) {
pwmWriteDshotCommand(index, DSHOT_CMD_BEEP3); pwmWriteDshotCommand(index, DSHOT_CMD_BEEP3);
} }

View file

@ -118,14 +118,14 @@ static const uint8_t ubloxInit[] = {
0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Pedistrian and 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Pedistrian and
0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // capturing the data from the U-Center binary console. 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // capturing the data from the U-Center binary console.
0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xC2, 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xC2,
//Preprocessor Airborne_1g Dynamic Platform Model Option //Preprocessor Airborne_1g Dynamic Platform Model Option
#elif defined(GPS_UBLOX_MODE_AIRBORNE_1G) #elif defined(GPS_UBLOX_MODE_AIRBORNE_1G)
0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, // CFG-NAV5 - Set engine settings 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, // CFG-NAV5 - Set engine settings
0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Airborne with 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Airborne with
0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // <1g acceleration and capturing the data from the U-Center binary console. 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // <1g acceleration and capturing the data from the U-Center binary console.
0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1A, 0x28, 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1A, 0x28,
//Default Airborne_4g Dynamic Platform Model //Default Airborne_4g Dynamic Platform Model
#else #else
0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x08, 0x03, 0x00, // CFG-NAV5 - Set engine settings 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x08, 0x03, 0x00, // CFG-NAV5 - Set engine settings
@ -133,7 +133,7 @@ static const uint8_t ubloxInit[] = {
0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // <4g acceleration and capturing the data from the U-Center binary console. 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // <4g acceleration and capturing the data from the U-Center binary console.
0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1C, 0x6C, 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1C, 0x6C,
#endif #endif
// DISABLE NMEA messages // DISABLE NMEA messages
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19, // VGS: Course over ground and Ground speed 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19, // VGS: Course over ground and Ground speed
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15, // GSV: GNSS Satellites in View 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15, // GSV: GNSS Satellites in View
@ -273,7 +273,7 @@ void gpsInitNmea(void)
#if defined(COLIBRI_RACE) || defined(LUX_RACE) #if defined(COLIBRI_RACE) || defined(LUX_RACE)
uint32_t now; uint32_t now;
#endif #endif
switch(gpsData.state) { switch (gpsData.state) {
case GPS_INITIALIZING: case GPS_INITIALIZING:
#if defined(COLIBRI_RACE) || defined(LUX_RACE) #if defined(COLIBRI_RACE) || defined(LUX_RACE)
now = millis(); now = millis();
@ -361,7 +361,7 @@ void gpsInitUblox(void)
case GPS_CONFIGURE: case GPS_CONFIGURE:
// Either use specific config file for GPS or let dynamically upload config // Either use specific config file for GPS or let dynamically upload config
if( gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF ) { if ( gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF ) {
gpsSetState(GPS_RECEIVING_DATA); gpsSetState(GPS_RECEIVING_DATA);
break; break;
} }
@ -628,7 +628,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:
@ -661,7 +661,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;
@ -671,7 +671,7 @@ static bool gpsNewFrameNMEA(char c)
} }
break; break;
case FRAME_GSV: case FRAME_GSV:
switch(param) { switch (param) {
/*case 1: /*case 1:
// Total number of messages of this type in this cycle // Total number of messages of this type in this cycle
break; */ break; */
@ -684,17 +684,17 @@ static bool gpsNewFrameNMEA(char c)
GPS_numCh = grab_fields(string, 0); GPS_numCh = grab_fields(string, 0);
break; break;
} }
if(param < 4) if (param < 4)
break; break;
svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4 svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4
svSatNum = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number svSatNum = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number
svSatParam = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite svSatParam = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite
if(svSatNum > GPS_SV_MAXSATS) if (svSatNum > GPS_SV_MAXSATS)
break; break;
switch(svSatParam) { switch (svSatParam) {
case 1: case 1:
// SV PRN number // SV PRN number
GPS_svinfo_chn[svSatNum - 1] = svSatNum; GPS_svinfo_chn[svSatNum - 1] = svSatNum;
@ -980,7 +980,7 @@ static bool UBLOX_parse_gps(void)
GPS_numCh = _buffer.svinfo.numCh; GPS_numCh = _buffer.svinfo.numCh;
if (GPS_numCh > 16) if (GPS_numCh > 16)
GPS_numCh = 16; GPS_numCh = 16;
for (i = 0; i < GPS_numCh; i++){ for (i = 0; i < GPS_numCh; i++) {
GPS_svinfo_chn[i]= _buffer.svinfo.channel[i].chn; GPS_svinfo_chn[i]= _buffer.svinfo.channel[i].chn;
GPS_svinfo_svid[i]= _buffer.svinfo.channel[i].svid; GPS_svinfo_svid[i]= _buffer.svinfo.channel[i].svid;
GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality; GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality;
@ -1103,7 +1103,7 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
waitForSerialPortToFinishTransmitting(gpsPort); waitForSerialPortToFinishTransmitting(gpsPort);
waitForSerialPortToFinishTransmitting(gpsPassthroughPort); waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
if(!(gpsPort->mode & MODE_TX)) if (!(gpsPort->mode & MODE_TX))
serialSetMode(gpsPort, gpsPort->mode | MODE_TX); serialSetMode(gpsPort, gpsPort->mode | MODE_TX);
#ifdef USE_DASHBOARD #ifdef USE_DASHBOARD

View file

@ -600,35 +600,35 @@ static void applyLedVtxLayer(bool updateNow, timeUs_t *timer)
// check if last vtx values have changed. // check if last vtx values have changed.
check = pit + (power << 1) + (band << 4) + (channel << 8); check = pit + (power << 1) + (band << 4) + (channel << 8);
if(!showSettings && check != lastCheck) { if (!showSettings && check != lastCheck) {
// display settings for 3 seconds. // display settings for 3 seconds.
showSettings = 15; showSettings = 15;
} }
lastCheck = check; // quick way to check if any settings changed. lastCheck = check; // quick way to check if any settings changed.
if(showSettings) { if (showSettings) {
showSettings--; showSettings--;
} }
blink = !blink; blink = !blink;
*timer += HZ_TO_US(5); // check 5 times a second *timer += HZ_TO_US(5); // check 5 times a second
} }
if(!active) { // no vtx device detected if (!active) { // no vtx device detected
return; return;
} }
hsvColor_t color = {0, 0, 0}; hsvColor_t color = {0, 0, 0};
if(showSettings) { // show settings if (showSettings) { // show settings
uint8_t vtxLedCount = 0; uint8_t vtxLedCount = 0;
for (int i = 0; i < ledCounts.count && vtxLedCount < 6; ++i) { for (int i = 0; i < ledCounts.count && vtxLedCount < 6; ++i) {
const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
if (ledGetOverlayBit(ledConfig, LED_OVERLAY_VTX)) { if (ledGetOverlayBit(ledConfig, LED_OVERLAY_VTX)) {
if(vtxLedCount == 0) { if (vtxLedCount == 0) {
color.h = HSV(GREEN).h; color.h = HSV(GREEN).h;
color.s = HSV(GREEN).s; color.s = HSV(GREEN).s;
color.v = blink ? 15 : 0; // blink received settings color.v = blink ? 15 : 0; // blink received settings
} }
else if(vtxLedCount > 0 && power >= vtxLedCount && !pit) { // show power else if (vtxLedCount > 0 && power >= vtxLedCount && !pit) { // show power
color.h = HSV(ORANGE).h; color.h = HSV(ORANGE).h;
color.s = HSV(ORANGE).s; color.s = HSV(ORANGE).s;
color.v = blink ? 15 : 0; // blink received settings color.v = blink ? 15 : 0; // blink received settings
@ -949,7 +949,7 @@ static void applyLedAnimationLayer(bool updateNow, timeUs_t *timer)
{ {
static uint8_t frameCounter = 0; static uint8_t frameCounter = 0;
const int animationFrames = ledGridRows; const int animationFrames = ledGridRows;
if(updateNow) { if (updateNow) {
frameCounter = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; frameCounter = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0;
*timer += HZ_TO_US(20); *timer += HZ_TO_US(20);
} }
@ -1087,7 +1087,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;
} }
@ -1128,7 +1128,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

@ -177,7 +177,7 @@ static int osdGetBatteryAverageCellVoltage(void)
static char osdGetBatterySymbol(int cellVoltage) static char osdGetBatterySymbol(int cellVoltage)
{ {
if(getBatteryState() == BATTERY_CRITICAL) { if (getBatteryState() == BATTERY_CRITICAL) {
return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark
} else { } else {
/* Calculate a symbol offset using cell voltage over full cell voltage range */ /* Calculate a symbol offset using cell voltage over full cell voltage range */
@ -532,7 +532,7 @@ static void osdDrawSingleElement(uint8_t item)
} }
/* Show battery state warning */ /* Show battery state warning */
switch(getBatteryState()) { switch (getBatteryState()) {
case BATTERY_WARNING: case BATTERY_WARNING:
tfp_sprintf(buff, "LOW BATTERY"); tfp_sprintf(buff, "LOW BATTERY");
break; break;
@ -586,7 +586,7 @@ static void osdDrawSingleElement(uint8_t item)
//Create empty battery indicator bar //Create empty battery indicator bar
buff[0] = SYM_PB_START; buff[0] = SYM_PB_START;
for(uint8_t i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) { for (uint8_t i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) {
if (i <= mAhUsedProgress) if (i <= mAhUsedProgress)
buff[i] = SYM_PB_FULL; buff[i] = SYM_PB_FULL;
else else

View file

@ -48,7 +48,7 @@ static uint8_t crc_high_first(uint8_t *ptr, uint8_t len)
uint8_t i; uint8_t i;
uint8_t crc=0x00; uint8_t crc=0x00;
while (len--) { while (len--) {
crc ^= *ptr++; crc ^= *ptr++;
for (i=8; i>0; --i) { for (i=8; i>0; --i) {
if (crc & 0x80) if (crc & 0x80)
crc = (crc << 1) ^ 0x31; crc = (crc << 1) ^ 0x31;
@ -111,7 +111,7 @@ static void rcSplitProcessMode()
argument = RCSPLIT_CTRL_ARGU_INVALID; argument = RCSPLIT_CTRL_ARGU_INVALID;
break; break;
} }
if (argument != RCSPLIT_CTRL_ARGU_INVALID) { if (argument != RCSPLIT_CTRL_ARGU_INVALID) {
sendCtrlCommand(argument); sendCtrlCommand(argument);
switchStates[switchIndex].isActivated = true; switchStates[switchIndex].isActivated = true;
@ -140,7 +140,7 @@ bool rcSplitInit(void)
uint8_t switchIndex = i - BOXCAMERA1; uint8_t switchIndex = i - BOXCAMERA1;
switchStates[switchIndex].isActivated = true; switchStates[switchIndex].isActivated = true;
} }
cameraState = RCSPLIT_STATE_IS_READY; cameraState = RCSPLIT_STATE_IS_READY;
#ifdef USE_RCSPLIT #ifdef USE_RCSPLIT

View file

@ -345,7 +345,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();
@ -536,7 +536,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

@ -310,7 +310,7 @@ uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) {
int i; int i;
crc = crc ^ ((uint16_t)data << 8); crc = crc ^ ((uint16_t)data << 8);
for (i=0; i < 8; i++){ for (i=0; i < 8; i++) {
if (crc & 0x8000) if (crc & 0x8000)
crc = (crc << 1) ^ 0x1021; crc = (crc << 1) ^ 0x1021;
else else
@ -432,7 +432,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;
@ -460,7 +460,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;
@ -475,12 +475,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:
@ -599,7 +599,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;
@ -611,7 +611,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:
{ {
@ -661,14 +661,14 @@ 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:
case imARM_BLB: case imARM_BLB:
{ {
if(!BL_ReadFlash(CurrentInterfaceMode, &ioMem)) if (!BL_ReadFlash(CurrentInterfaceMode, &ioMem))
{ {
ACK_OUT = ACK_D_GENERAL_ERROR; ACK_OUT = ACK_D_GENERAL_ERROR;
} }
@ -678,7 +678,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;
} }
@ -729,7 +729,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

@ -86,7 +86,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);
@ -109,8 +109,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 {
@ -148,22 +148,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);
@ -252,7 +252,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);
@ -294,7 +294,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

@ -176,9 +176,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;
@ -189,7 +189,7 @@ static uint8_t StkReadLeader(void)
} }
//Skip the first bits //Skip the first bits
if (waitcycl == 0){ if (waitcycl == 0) {
goto timeout; goto timeout;
} }
@ -271,7 +271,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

@ -226,7 +226,7 @@ void handleVTXControlButton(void)
LED1_OFF; LED1_OFF;
switch(actionCounter) { switch (actionCounter) {
case 4: case 4:
vtxCycleBandOrChannel(0, +1); vtxCycleBandOrChannel(0, +1);
break; break;

View file

@ -332,7 +332,7 @@ static void saProcessResponse(uint8_t *buf, int len)
dprintf(("processResponse: outstanding %d got %d\r\n", sa_outstanding, resp)); dprintf(("processResponse: outstanding %d got %d\r\n", sa_outstanding, resp));
} }
switch(resp) { switch (resp) {
case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings
case SA_CMD_GET_SETTINGS: // Version 1 Get Settings case SA_CMD_GET_SETTINGS: // Version 1 Get Settings
if (len < 7) if (len < 7)
@ -420,7 +420,7 @@ static void saReceiveFramer(uint8_t c)
static int len; static int len;
static int dlen; static int dlen;
switch(state) { switch (state) {
case S_WAITPRE1: case S_WAITPRE1:
if (c == 0xAA) { if (c == 0xAA) {
state = S_WAITPRE2; state = S_WAITPRE2;

View file

@ -134,7 +134,7 @@ void trampCmdU16(uint8_t cmd, uint16_t param)
void trampSetFreq(uint16_t freq) void trampSetFreq(uint16_t freq)
{ {
trampConfFreq = freq; trampConfFreq = freq;
if(trampConfFreq != trampCurFreq) if (trampConfFreq != trampCurFreq)
trampFreqRetries = TRAMP_MAX_RETRIES; trampFreqRetries = TRAMP_MAX_RETRIES;
} }
@ -151,7 +151,7 @@ void trampSetBandAndChannel(uint8_t band, uint8_t channel)
void trampSetRFPower(uint16_t level) void trampSetRFPower(uint16_t level)
{ {
trampConfPower = level; trampConfPower = level;
if(trampConfPower != trampPower) if (trampConfPower != trampPower)
trampPowerRetries = TRAMP_MAX_RETRIES; trampPowerRetries = TRAMP_MAX_RETRIES;
} }
@ -163,7 +163,7 @@ void trampSendRFPower(uint16_t level)
// return false if error // return false if error
bool trampCommitChanges() bool trampCommitChanges()
{ {
if(trampStatus != TRAMP_STATUS_ONLINE) if (trampStatus != TRAMP_STATUS_ONLINE)
return false; return false;
trampStatus = TRAMP_STATUS_SET_FREQ_PW; trampStatus = TRAMP_STATUS_SET_FREQ_PW;
@ -184,7 +184,7 @@ char trampHandleResponse(void)
case 'r': case 'r':
{ {
uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
if(min_freq != 0) { if (min_freq != 0) {
trampRFFreqMin = min_freq; trampRFFreqMin = min_freq;
trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8); trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8); trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8);
@ -198,15 +198,15 @@ char trampHandleResponse(void)
case 'v': case 'v':
{ {
uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
if(freq != 0) { if (freq != 0) {
trampCurFreq = freq; trampCurFreq = freq;
trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
trampPitMode = trampRespBuffer[7]; trampPitMode = trampRespBuffer[7];
trampPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8); trampPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
vtx58_Freq2Bandchan(trampCurFreq, &trampBand, &trampChannel); vtx58_Freq2Bandchan(trampCurFreq, &trampBand, &trampChannel);
if(trampConfFreq == 0) trampConfFreq = trampCurFreq; if (trampConfFreq == 0) trampConfFreq = trampCurFreq;
if(trampConfPower == 0) trampConfPower = trampPower; if (trampConfPower == 0) trampConfPower = trampPower;
return 'v'; return 'v';
} }
@ -217,7 +217,7 @@ char trampHandleResponse(void)
case 's': case 's':
{ {
uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
if(temp != 0) { if (temp != 0) {
trampTemperature = temp; trampTemperature = temp;
return 's'; return 's';
} }
@ -263,7 +263,7 @@ static char trampReceive(uint32_t currentTimeUs)
uint8_t c = serialRead(trampSerialPort); uint8_t c = serialRead(trampSerialPort);
trampRespBuffer[trampReceivePos++] = c; trampRespBuffer[trampReceivePos++] = c;
switch(trampReceiveState) { switch (trampReceiveState) {
case S_WAIT_LEN: case S_WAIT_LEN:
if (c == 0x0F) { if (c == 0x0F) {
trampReceiveState = S_WAIT_CODE; trampReceiveState = S_WAIT_CODE;
@ -339,7 +339,7 @@ void vtxTrampProcess(uint32_t currentTimeUs)
debug[0] = trampStatus; debug[0] = trampStatus;
#endif #endif
switch(replyCode) { switch (replyCode) {
case 'r': case 'r':
if (trampStatus <= TRAMP_STATUS_OFFLINE) if (trampStatus <= TRAMP_STATUS_OFFLINE)
trampStatus = TRAMP_STATUS_ONLINE; trampStatus = TRAMP_STATUS_ONLINE;
@ -351,7 +351,7 @@ void vtxTrampProcess(uint32_t currentTimeUs)
break; break;
} }
switch(trampStatus) { switch (trampStatus) {
case TRAMP_STATUS_OFFLINE: case TRAMP_STATUS_OFFLINE:
case TRAMP_STATUS_ONLINE: case TRAMP_STATUS_ONLINE:
@ -361,7 +361,7 @@ void vtxTrampProcess(uint32_t currentTimeUs)
trampQueryR(); trampQueryR();
else { else {
static unsigned int cnt = 0; static unsigned int cnt = 0;
if(((cnt++) & 1) == 0) if (((cnt++) & 1) == 0)
trampQueryV(); trampQueryV();
else else
trampQueryS(); trampQueryS();
@ -391,7 +391,7 @@ void vtxTrampProcess(uint32_t currentTimeUs)
done = false; done = false;
} }
if(!done) { if (!done) {
trampStatus = TRAMP_STATUS_CHECK_FREQ_PW; trampStatus = TRAMP_STATUS_CHECK_FREQ_PW;
// delay next status query by 300ms // delay next status query by 300ms
@ -560,8 +560,8 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self)
static void trampCmsInitSettings() static void trampCmsInitSettings()
{ {
if(trampBand > 0) trampCmsBand = trampBand; if (trampBand > 0) trampCmsBand = trampBand;
if(trampChannel > 0) trampCmsChan = trampChannel; if (trampChannel > 0) trampCmsChan = trampChannel;
trampCmsUpdateFreqRef(); trampCmsUpdateFreqRef();
trampCmsPitMode = trampPitMode + 1; trampCmsPitMode = trampPitMode + 1;
@ -725,7 +725,7 @@ bool vtxTrampInit(void)
#if defined(VTX_COMMON) #if defined(VTX_COMMON)
vtxCommonRegisterDevice(&vtxTramp); vtxCommonRegisterDevice(&vtxTramp);
#endif #endif
return true; return true;
} }

View file

@ -83,7 +83,7 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
mspPort->c_state = (c == 'M') ? MSP_HEADER_M : MSP_IDLE; mspPort->c_state = (c == 'M') ? MSP_HEADER_M : MSP_IDLE;
} else if (mspPort->c_state == MSP_HEADER_M) { } else if (mspPort->c_state == MSP_HEADER_M) {
mspPort->c_state = MSP_IDLE; mspPort->c_state = MSP_IDLE;
switch(c) { switch (c) {
case '<': // COMMAND case '<': // COMMAND
mspPort->packetType = MSP_PACKET_COMMAND; mspPort->packetType = MSP_PACKET_COMMAND;
mspPort->c_state = MSP_HEADER_ARROW; mspPort->c_state = MSP_HEADER_ARROW;

View file

@ -209,10 +209,10 @@ void init(void)
#endif #endif
#ifdef USE_I2C_DEVICE_3 #ifdef USE_I2C_DEVICE_3
i2cInit(I2CDEV_3); i2cInit(I2CDEV_3);
#endif #endif
#ifdef USE_I2C_DEVICE_4 #ifdef USE_I2C_DEVICE_4
i2cInit(I2CDEV_4); i2cInit(I2CDEV_4);
#endif #endif
#endif /* USE_I2C */ #endif /* USE_I2C */
#endif /* TARGET_BUS_INIT */ #endif /* TARGET_BUS_INIT */

View file

@ -231,7 +231,7 @@ uint16_t calcCRC16(uint8_t *pt, uint8_t msgLen)
uint16_t crc16_data = 0; uint16_t crc16_data = 0;
uint8_t data=0; uint8_t data=0;
for (uint8_t mlen = 0; mlen < msgLen; mlen++){ for (uint8_t mlen = 0; mlen < msgLen; mlen++) {
data = pt[mlen] ^ ((uint8_t)(crc16_data) & (uint8_t)(0xFF)); data = pt[mlen] ^ ((uint8_t)(crc16_data) & (uint8_t)(0xFF));
data ^= data << 4; data ^= data << 4;
crc16_data = ((((uint16_t)data << 8) | ((crc16_data & 0xFF00) >> 8)) crc16_data = ((((uint16_t)data << 8) | ((crc16_data & 0xFF00) >> 8))
@ -265,7 +265,7 @@ void jetiExBusDecodeChannelFrame(uint8_t *exBusFrame)
uint8_t frameAddr; uint8_t frameAddr;
// Decode header // Decode header
switch (((uint16_t)exBusFrame[EXBUS_HEADER_SYNC] << 8) | ((uint16_t)exBusFrame[EXBUS_HEADER_REQ])){ switch (((uint16_t)exBusFrame[EXBUS_HEADER_SYNC] << 8) | ((uint16_t)exBusFrame[EXBUS_HEADER_REQ])) {
case EXBUS_CHANNELDATA_DATA_REQUEST: // not yet specified case EXBUS_CHANNELDATA_DATA_REQUEST: // not yet specified
case EXBUS_CHANNELDATA: case EXBUS_CHANNELDATA:
@ -322,7 +322,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;
@ -345,12 +345,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;
} }
@ -381,7 +381,7 @@ static 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;
@ -453,18 +453,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--;
@ -472,9 +472,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;
} }
@ -517,13 +517,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 = getBatteryVoltage(); jetiExSensors[EX_VOLTAGE].value = getBatteryVoltage();
jetiExSensors[EX_CURRENT].value = getAmperage(); jetiExSensors[EX_CURRENT].value = getAmperage();
jetiExSensors[EX_ALTITUDE].value = getEstimatedAltitude(); jetiExSensors[EX_ALTITUDE].value = getEstimatedAltitude();
@ -562,7 +562,7 @@ void sendJetiExBusTelemetry(uint8_t packetID)
static uint8_t requestLoop = 0; static uint8_t requestLoop = 0;
uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA]; uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA];
if (requestLoop == 100){ //every nth request send the name of a value if (requestLoop == 100) { //every nth request send the name of a value
if (sensorDescriptionCounter == JETI_EX_SENSOR_COUNT ) if (sensorDescriptionCounter == JETI_EX_SENSOR_COUNT )
sensorDescriptionCounter = 0; sensorDescriptionCounter = 0;

View file

@ -153,7 +153,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

@ -457,7 +457,7 @@ static uint16_t getRxfailValue(uint8_t channel)
{ {
const rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigs(channel); const rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigs(channel);
switch(channelFailsafeConfig->mode) { switch (channelFailsafeConfig->mode) {
case RX_FAILSAFE_MODE_AUTO: case RX_FAILSAFE_MODE_AUTO:
switch (channel) { switch (channel) {
case ROLL: case ROLL:

View file

@ -126,11 +126,11 @@ static 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
@ -144,7 +144,7 @@ static 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

@ -225,7 +225,7 @@ retry:
#ifdef ACC_MPU6500_ALIGN #ifdef ACC_MPU6500_ALIGN
dev->accAlign = ACC_MPU6500_ALIGN; dev->accAlign = ACC_MPU6500_ALIGN;
#endif #endif
switch(dev->mpuDetectionResult.sensor) { switch (dev->mpuDetectionResult.sensor) {
case MPU_9250_SPI: case MPU_9250_SPI:
accHardware = ACC_MPU9250; accHardware = ACC_MPU9250;
break; break;

View file

@ -110,7 +110,7 @@ void batteryUpdateVoltage(timeUs_t currentTimeUs)
{ {
UNUSED(currentTimeUs); UNUSED(currentTimeUs);
switch(batteryConfig()->voltageMeterSource) { switch (batteryConfig()->voltageMeterSource) {
#ifdef USE_ESC_SENSOR #ifdef USE_ESC_SENSOR
case VOLTAGE_METER_ESC: case VOLTAGE_METER_ESC:
if (feature(FEATURE_ESC_SENSOR)) { if (feature(FEATURE_ESC_SENSOR)) {
@ -138,7 +138,7 @@ void batteryUpdateVoltage(timeUs_t currentTimeUs)
static void updateBatteryBeeperAlert(void) static void updateBatteryBeeperAlert(void)
{ {
switch(getBatteryState()) { switch (getBatteryState()) {
case BATTERY_WARNING: case BATTERY_WARNING:
beeper(BEEPER_BAT_LOW); beeper(BEEPER_BAT_LOW);
@ -158,12 +158,12 @@ void batteryUpdatePresence(void)
static uint16_t previousVoltage = 0; static uint16_t previousVoltage = 0;
bool isVoltageStable = ABS(voltageMeter.filtered - previousVoltage) <= VBAT_STABLE_MAX_DELTA; bool isVoltageStable = ABS(voltageMeter.filtered - previousVoltage) <= VBAT_STABLE_MAX_DELTA;
bool isVoltageFromBat = (voltageMeter.filtered >= batteryConfig()->vbatnotpresentcellvoltage //above ~0V bool isVoltageFromBat = (voltageMeter.filtered >= batteryConfig()->vbatnotpresentcellvoltage //above ~0V
&& voltageMeter.filtered <= batteryConfig()->vbatmaxcellvoltage) //1s max cell voltage check && voltageMeter.filtered <= batteryConfig()->vbatmaxcellvoltage) //1s max cell voltage check
|| voltageMeter.filtered > batteryConfig()->vbatnotpresentcellvoltage*2; //USB voltage - 2s or more check || voltageMeter.filtered > batteryConfig()->vbatnotpresentcellvoltage*2; //USB voltage - 2s or more check
if ( if (
voltageState == BATTERY_NOT_PRESENT voltageState == BATTERY_NOT_PRESENT
&& isVoltageFromBat && isVoltageFromBat
@ -208,7 +208,7 @@ void batteryUpdatePresence(void)
static void batteryUpdateVoltageState(void) static void batteryUpdateVoltageState(void)
{ {
// alerts are currently used by beeper, osd and other subsystems // alerts are currently used by beeper, osd and other subsystems
switch(voltageState) { switch (voltageState) {
case BATTERY_OK: case BATTERY_OK:
if (voltageMeter.filtered <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) { if (voltageMeter.filtered <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) {
voltageState = BATTERY_WARNING; voltageState = BATTERY_WARNING;
@ -270,7 +270,7 @@ void batteryInit(void)
batteryCriticalVoltage = 0; batteryCriticalVoltage = 0;
voltageMeterReset(&voltageMeter); voltageMeterReset(&voltageMeter);
switch(batteryConfig()->voltageMeterSource) { switch (batteryConfig()->voltageMeterSource) {
case VOLTAGE_METER_ESC: case VOLTAGE_METER_ESC:
#ifdef USE_ESC_SENSOR #ifdef USE_ESC_SENSOR
voltageMeterESCInit(); voltageMeterESCInit();
@ -290,7 +290,7 @@ void batteryInit(void)
// //
consumptionState = BATTERY_OK; consumptionState = BATTERY_OK;
currentMeterReset(&currentMeter); currentMeterReset(&currentMeter);
switch(batteryConfig()->currentMeterSource) { switch (batteryConfig()->currentMeterSource) {
case CURRENT_METER_ADC: case CURRENT_METER_ADC:
currentMeterADCInit(); currentMeterADCInit();
break; break;
@ -345,7 +345,7 @@ void batteryUpdateCurrentMeter(timeUs_t currentTimeUs)
const int32_t lastUpdateAt = cmp32(currentTimeUs, ibatLastServiced); const int32_t lastUpdateAt = cmp32(currentTimeUs, ibatLastServiced);
ibatLastServiced = currentTimeUs; ibatLastServiced = currentTimeUs;
switch(batteryConfig()->currentMeterSource) { switch (batteryConfig()->currentMeterSource) {
case CURRENT_METER_ADC: case CURRENT_METER_ADC:
currentMeterADCRefresh(lastUpdateAt); currentMeterADCRefresh(lastUpdateAt);
currentMeterADCRead(&currentMeter); currentMeterADCRead(&currentMeter);

View file

@ -77,7 +77,7 @@ retry:
dev->magAlign = ALIGN_DEFAULT; dev->magAlign = ALIGN_DEFAULT;
switch(magHardwareToUse) { switch (magHardwareToUse) {
case MAG_DEFAULT: case MAG_DEFAULT:
; // fallthrough ; // fallthrough

View file

@ -215,7 +215,7 @@ static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen) static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen)
{ {
uint8_t crc = 0; uint8_t crc = 0;
for(int i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc); for (int i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc);
return (crc); return (crc);
} }

View file

@ -155,7 +155,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev)
dev->gyroAlign = ALIGN_DEFAULT; dev->gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) { switch (gyroHardware) {
case GYRO_DEFAULT: case GYRO_DEFAULT:
#ifdef USE_GYRO_MPU6050 #ifdef USE_GYRO_MPU6050
case GYRO_MPU6050: case GYRO_MPU6050:
@ -222,7 +222,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev)
#else #else
if (mpu6500GyroDetect(dev)) { if (mpu6500GyroDetect(dev)) {
#endif #endif
switch(dev->mpuDetectionResult.sensor) { switch (dev->mpuDetectionResult.sensor) {
case MPU_9250_SPI: case MPU_9250_SPI:
gyroHardware = GYRO_MPU9250; gyroHardware = GYRO_MPU9250;
break; break;

View file

@ -80,9 +80,9 @@ uint8_t interruptCounter = 0;
void bstProcessInCommand(void); void bstProcessInCommand(void);
void I2C_EV_IRQHandler() void I2C_EV_IRQHandler()
{ {
if(I2C_GetITStatus(BSTx, I2C_IT_ADDR)) { if (I2C_GetITStatus(BSTx, I2C_IT_ADDR)) {
CRC8 = 0; CRC8 = 0;
if(I2C_GetTransferDirection(BSTx) == I2C_Direction_Receiver) { if (I2C_GetTransferDirection(BSTx) == I2C_Direction_Receiver) {
currentWriteBufferPointer = 0; currentWriteBufferPointer = 0;
receiverAddress = true; receiverAddress = true;
I2C_SendData(BSTx, (uint8_t) writeData[currentWriteBufferPointer++]); I2C_SendData(BSTx, (uint8_t) writeData[currentWriteBufferPointer++]);
@ -92,11 +92,11 @@ void I2C_EV_IRQHandler()
bufferPointer = 1; bufferPointer = 1;
} }
I2C_ClearITPendingBit(BSTx, I2C_IT_ADDR); I2C_ClearITPendingBit(BSTx, I2C_IT_ADDR);
} else if(I2C_GetITStatus(BSTx, I2C_IT_RXNE)) { } else if (I2C_GetITStatus(BSTx, I2C_IT_RXNE)) {
uint8_t data = I2C_ReceiveData(BSTx); uint8_t data = I2C_ReceiveData(BSTx);
readData[bufferPointer] = data; readData[bufferPointer] = data;
if(bufferPointer > 1) { if (bufferPointer > 1) {
if(readData[1]+1 == bufferPointer) { if (readData[1]+1 == bufferPointer) {
crc8Cal(0); crc8Cal(0);
bstProcessInCommand(); bstProcessInCommand();
} else { } else {
@ -105,21 +105,21 @@ void I2C_EV_IRQHandler()
} }
bufferPointer++; bufferPointer++;
I2C_ClearITPendingBit(BSTx, I2C_IT_RXNE); I2C_ClearITPendingBit(BSTx, I2C_IT_RXNE);
} else if(I2C_GetITStatus(BSTx, I2C_IT_TXIS)) { } else if (I2C_GetITStatus(BSTx, I2C_IT_TXIS)) {
if(receiverAddress) { if (receiverAddress) {
if(currentWriteBufferPointer > 0) { if (currentWriteBufferPointer > 0) {
if(!cleanflight_data_ready) { if (!cleanflight_data_ready) {
I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS); I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS);
return; return;
} }
if(interruptCounter < DELAY_SENDING_BYTE) { if (interruptCounter < DELAY_SENDING_BYTE) {
interruptCounter++; interruptCounter++;
I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS); I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS);
return; return;
} else { } else {
interruptCounter = 0; interruptCounter = 0;
} }
if(writeData[0] == currentWriteBufferPointer) { if (writeData[0] == currentWriteBufferPointer) {
receiverAddress = false; receiverAddress = false;
crc8Cal(0); crc8Cal(0);
I2C_SendData(BSTx, (uint8_t) CRC8); I2C_SendData(BSTx, (uint8_t) CRC8);
@ -129,11 +129,11 @@ void I2C_EV_IRQHandler()
I2C_SendData(BSTx, (uint8_t) writeData[currentWriteBufferPointer++]); I2C_SendData(BSTx, (uint8_t) writeData[currentWriteBufferPointer++]);
} }
} }
} else if(bstWriteDataLen) { } else if (bstWriteDataLen) {
I2C_SendData(BSTx, (uint8_t) dataBuffer[dataBufferPointer]); I2C_SendData(BSTx, (uint8_t) dataBuffer[dataBufferPointer]);
if(bstWriteDataLen > 1) if (bstWriteDataLen > 1)
dataBufferPointer++; dataBufferPointer++;
if(dataBufferPointer == bstWriteDataLen) { if (dataBufferPointer == bstWriteDataLen) {
I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE); I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE);
dataBufferPointer = 0; dataBufferPointer = 0;
bstWriteDataLen = 0; bstWriteDataLen = 0;
@ -141,19 +141,19 @@ void I2C_EV_IRQHandler()
} else { } else {
} }
I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS); I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS);
} else if(I2C_GetITStatus(BSTx, I2C_IT_NACKF)) { } else if (I2C_GetITStatus(BSTx, I2C_IT_NACKF)) {
if(receiverAddress) { if (receiverAddress) {
receiverAddress = false; receiverAddress = false;
I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE); I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE);
} }
I2C_ClearITPendingBit(BSTx, I2C_IT_NACKF); I2C_ClearITPendingBit(BSTx, I2C_IT_NACKF);
} else if(I2C_GetITStatus(BSTx, I2C_IT_STOPF)) { } else if (I2C_GetITStatus(BSTx, I2C_IT_STOPF)) {
if(bstWriteDataLen && dataBufferPointer == bstWriteDataLen) { if (bstWriteDataLen && dataBufferPointer == bstWriteDataLen) {
dataBufferPointer = 0; dataBufferPointer = 0;
bstWriteDataLen = 0; bstWriteDataLen = 0;
} }
I2C_ClearITPendingBit(BSTx, I2C_IT_STOPF); I2C_ClearITPendingBit(BSTx, I2C_IT_STOPF);
} else if(I2C_GetITStatus(BSTx, I2C_IT_BERR) } else if (I2C_GetITStatus(BSTx, I2C_IT_BERR)
|| I2C_GetITStatus(BSTx, I2C_IT_ARLO) || I2C_GetITStatus(BSTx, I2C_IT_ARLO)
|| I2C_GetITStatus(BSTx, I2C_IT_OVR)) { || I2C_GetITStatus(BSTx, I2C_IT_OVR)) {
bstTimeoutUserCallback(); bstTimeoutUserCallback();
@ -193,7 +193,7 @@ void bstInitPort(I2C_TypeDef *BSTx/*, uint8_t Address*/)
GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitTypeDef GPIO_InitStructure;
I2C_InitTypeDef BST_InitStructure; I2C_InitTypeDef BST_InitStructure;
if(BSTx == I2C1) { if (BSTx == I2C1) {
RCC_AHBPeriphClockCmd(BST1_SCL_CLK_SOURCE | BST1_SDA_CLK_SOURCE, ENABLE); RCC_AHBPeriphClockCmd(BST1_SCL_CLK_SOURCE | BST1_SDA_CLK_SOURCE, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
RCC_I2CCLKConfig(RCC_I2C1CLK_SYSCLK); RCC_I2CCLKConfig(RCC_I2C1CLK_SYSCLK);
@ -241,7 +241,7 @@ void bstInitPort(I2C_TypeDef *BSTx/*, uint8_t Address*/)
I2C_Cmd(I2C1, ENABLE); I2C_Cmd(I2C1, ENABLE);
} }
if(BSTx == I2C2) { if (BSTx == I2C2) {
RCC_AHBPeriphClockCmd(BST2_SCL_CLK_SOURCE | BST2_SDA_CLK_SOURCE, ENABLE); RCC_AHBPeriphClockCmd(BST2_SCL_CLK_SOURCE | BST2_SDA_CLK_SOURCE, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
RCC_I2CCLKConfig(RCC_I2C2CLK_SYSCLK); RCC_I2CCLKConfig(RCC_I2C2CLK_SYSCLK);
@ -314,7 +314,7 @@ uint16_t bstGetErrorCounter(void)
bool bstWriteBusy(void) bool bstWriteBusy(void)
{ {
if(bstWriteDataLen) if (bstWriteDataLen)
return true; return true;
else else
return false; return false;
@ -322,14 +322,14 @@ bool bstWriteBusy(void)
bool bstMasterWrite(uint8_t* data) bool bstMasterWrite(uint8_t* data)
{ {
if(bstWriteDataLen==0) { if (bstWriteDataLen==0) {
CRC8 = 0; CRC8 = 0;
dataBufferPointer = 0; dataBufferPointer = 0;
dataBuffer[0] = *data; dataBuffer[0] = *data;
dataBuffer[1] = *(data+1); dataBuffer[1] = *(data+1);
bstWriteDataLen = dataBuffer[1] + 2; bstWriteDataLen = dataBuffer[1] + 2;
for(uint8_t i=2; i<bstWriteDataLen; i++) { for (uint8_t i=2; i<bstWriteDataLen; i++) {
if(i==(bstWriteDataLen-1)) { if (i==(bstWriteDataLen-1)) {
crc8Cal(0); crc8Cal(0);
dataBuffer[i] = CRC8; dataBuffer[i] = CRC8;
} else { } else {
@ -346,19 +346,19 @@ void bstMasterWriteLoop(void)
{ {
static uint32_t bstMasterWriteTimeout = 0; static uint32_t bstMasterWriteTimeout = 0;
uint32_t currentTime = micros(); uint32_t currentTime = micros();
if(bstWriteDataLen && dataBufferPointer==0) { if (bstWriteDataLen && dataBufferPointer==0) {
bool scl_set = false; bool scl_set = false;
if(BSTx == I2C1) if (BSTx == I2C1)
scl_set = BST1_SCL_GPIO->IDR&BST1_SCL_PIN; scl_set = BST1_SCL_GPIO->IDR&BST1_SCL_PIN;
else else
scl_set = BST2_SCL_GPIO->IDR&BST2_SCL_PIN; scl_set = BST2_SCL_GPIO->IDR&BST2_SCL_PIN;
if(I2C_GetFlagStatus(BSTx, I2C_FLAG_BUSY)==RESET && scl_set) { if (I2C_GetFlagStatus(BSTx, I2C_FLAG_BUSY)==RESET && scl_set) {
I2C_TransferHandling(BSTx, dataBuffer[dataBufferPointer], dataBuffer[dataBufferPointer+1]+1, I2C_AutoEnd_Mode, I2C_Generate_Start_Write); I2C_TransferHandling(BSTx, dataBuffer[dataBufferPointer], dataBuffer[dataBufferPointer+1]+1, I2C_AutoEnd_Mode, I2C_Generate_Start_Write);
I2C_ITConfig(BSTx, I2C_IT_TXI, ENABLE); I2C_ITConfig(BSTx, I2C_IT_TXI, ENABLE);
dataBufferPointer = 1; dataBufferPointer = 1;
bstMasterWriteTimeout = micros(); bstMasterWriteTimeout = micros();
} }
} else if(currentTime>bstMasterWriteTimeout+BST_SHORT_TIMEOUT) { } else if (currentTime>bstMasterWriteTimeout+BST_SHORT_TIMEOUT) {
bstTimeoutUserCallback(); bstTimeoutUserCallback();
} }
} }

View file

@ -269,7 +269,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
{ {
uint32_t i, tmp, junk; uint32_t i, tmp, junk;
switch(bstRequest) { switch (bstRequest) {
case BST_API_VERSION: case BST_API_VERSION:
bstWrite8(BST_PROTOCOL_VERSION); bstWrite8(BST_PROTOCOL_VERSION);
@ -455,7 +455,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
uint16_t tmp; uint16_t tmp;
bool ret = BST_PASSED; bool ret = BST_PASSED;
switch(bstWriteCommand) { switch (bstWriteCommand) {
case BST_SELECT_SETTING: case BST_SELECT_SETTING:
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
changePidProfile(bstRead8()); changePidProfile(bstRead8());
@ -620,7 +620,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
} }
bstWrite8(ret); bstWrite8(ret);
if(ret == BST_FAILED) if (ret == BST_FAILED)
return false; return false;
return true; return true;
@ -651,18 +651,18 @@ extern bool cleanflight_data_ready;
void bstProcessInCommand(void) void bstProcessInCommand(void)
{ {
readBufferPointer = 2; readBufferPointer = 2;
if(bstCurrentAddress() == I2C_ADDR_CLEANFLIGHT_FC) { if (bstCurrentAddress() == I2C_ADDR_CLEANFLIGHT_FC) {
if(bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) { if (bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) {
uint8_t i; uint8_t i;
writeBufferPointer = 1; writeBufferPointer = 1;
cleanflight_data_ready = false; cleanflight_data_ready = false;
for(i = 0; i < BST_BUFFER_SIZE; i++) { for (i = 0; i < BST_BUFFER_SIZE; i++) {
writeData[i] = 0; writeData[i] = 0;
} }
switch (bstRead8()) { switch (bstRead8()) {
case BST_USB_DEVICE_INFO_REQUEST: case BST_USB_DEVICE_INFO_REQUEST:
bstRead8(); bstRead8();
if(bstSlaveUSBCommandFeedback(/*bstRead8()*/)) if (bstSlaveUSBCommandFeedback(/*bstRead8()*/))
coreProReady = true; coreProReady = true;
break; break;
case BST_READ_COMMANDS: case BST_READ_COMMANDS:
@ -679,8 +679,8 @@ void bstProcessInCommand(void)
} }
cleanflight_data_ready = true; cleanflight_data_ready = true;
} }
} else if(bstCurrentAddress() == 0x00) { } else if (bstCurrentAddress() == 0x00) {
if(bstReadCRC() == CRC8 && bstRead8()==BST_GENERAL_HEARTBEAT) { if (bstReadCRC() == CRC8 && bstRead8()==BST_GENERAL_HEARTBEAT) {
resetBstTimer = micros(); resetBstTimer = micros();
needResetCheck = true; needResetCheck = true;
} }
@ -689,8 +689,8 @@ void bstProcessInCommand(void)
static void resetBstChecker(timeUs_t currentTimeUs) static void resetBstChecker(timeUs_t currentTimeUs)
{ {
if(needResetCheck) { if (needResetCheck) {
if(currentTimeUs >= (resetBstTimer + BST_RESET_TIME)) if (currentTimeUs >= (resetBstTimer + BST_RESET_TIME))
{ {
bstTimeoutUserCallback(); bstTimeoutUserCallback();
needResetCheck = false; needResetCheck = false;
@ -709,23 +709,23 @@ static uint8_t sendCounter = 0;
void taskBstMasterProcess(timeUs_t currentTimeUs) void taskBstMasterProcess(timeUs_t currentTimeUs)
{ {
if(coreProReady) { if (coreProReady) {
if(currentTimeUs >= next02hzUpdateAt_1 && !bstWriteBusy()) { if (currentTimeUs >= next02hzUpdateAt_1 && !bstWriteBusy()) {
writeFCModeToBST(); writeFCModeToBST();
next02hzUpdateAt_1 = currentTimeUs + UPDATE_AT_02HZ; next02hzUpdateAt_1 = currentTimeUs + UPDATE_AT_02HZ;
} }
if(currentTimeUs >= next20hzUpdateAt_1 && !bstWriteBusy()) { if (currentTimeUs >= next20hzUpdateAt_1 && !bstWriteBusy()) {
if(sendCounter == 0) if (sendCounter == 0)
writeRCChannelToBST(); writeRCChannelToBST();
else if(sendCounter == 1) else if (sendCounter == 1)
writeRollPitchYawToBST(); writeRollPitchYawToBST();
sendCounter++; sendCounter++;
if(sendCounter > 1) if (sendCounter > 1)
sendCounter = 0; sendCounter = 0;
next20hzUpdateAt_1 = currentTimeUs + UPDATE_AT_20HZ; next20hzUpdateAt_1 = currentTimeUs + UPDATE_AT_20HZ;
} }
#ifdef GPS #ifdef GPS
if(sensors(SENSOR_GPS) && !bstWriteBusy()) if (sensors(SENSOR_GPS) && !bstWriteBusy())
writeGpsPositionPrameToBST(); writeGpsPositionPrameToBST();
#endif #endif
@ -779,7 +779,7 @@ static uint8_t numOfSat = 0;
#ifdef GPS #ifdef GPS
bool writeGpsPositionPrameToBST(void) bool writeGpsPositionPrameToBST(void)
{ {
if((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (alt != gpsSol.llh.alt) || (numOfSat != gpsSol.numSat)) { if ((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (alt != gpsSol.llh.alt) || (numOfSat != gpsSol.numSat)) {
lat = gpsSol.llh.lat; lat = gpsSol.llh.lat;
lon = gpsSol.llh.lon; lon = gpsSol.llh.lon;
alt = gpsSol.llh.alt; alt = gpsSol.llh.alt;
@ -826,7 +826,7 @@ bool writeRCChannelToBST(void)
uint8_t i = 0; uint8_t i = 0;
bstMasterStartBuffer(PUBLIC_ADDRESS); bstMasterStartBuffer(PUBLIC_ADDRESS);
bstMasterWrite8(RC_CHANNEL_FRAME_ID); bstMasterWrite8(RC_CHANNEL_FRAME_ID);
for(i = 0; i < (USABLE_TIMER_CHANNEL_COUNT-1); i++) { for (i = 0; i < (USABLE_TIMER_CHANNEL_COUNT-1); i++) {
bstMasterWrite16(rcData[i]); bstMasterWrite16(rcData[i]);
} }

View file

@ -68,10 +68,10 @@ static bool rcFrameComplete = false;
static void routeIncommingPacket(syslinkPacket_t* slp) static void routeIncommingPacket(syslinkPacket_t* slp)
{ {
// Only support packets of type SYSLINK_RADIO_RAW // Only support packets of type SYSLINK_RADIO_RAW
if(slp->type == SYSLINK_RADIO_RAW) { if (slp->type == SYSLINK_RADIO_RAW) {
crtpPacket_t *crtpPacket = (crtpPacket_t*)(slp->data); crtpPacket_t *crtpPacket = (crtpPacket_t*)(slp->data);
switch(crtpPacket->header.port) { switch (crtpPacket->header.port) {
case CRTP_PORT_SETPOINT: case CRTP_PORT_SETPOINT:
{ {
crtpCommanderRPYT_t *crtpRYPTPacket = crtpCommanderRPYT_t *crtpRYPTPacket =
@ -95,7 +95,7 @@ static void routeIncommingPacket(syslinkPacket_t* slp)
case CRTP_PORT_SETPOINT_GENERIC: case CRTP_PORT_SETPOINT_GENERIC:
// First byte of the packet is the type // First byte of the packet is the type
// Only support the CPPM Emulation type // Only support the CPPM Emulation type
if(crtpPacket->data[0] == cppmEmuType) { if (crtpPacket->data[0] == cppmEmuType) {
crtpCommanderCPPMEmuPacket_t *crtpCppmPacket = crtpCommanderCPPMEmuPacket_t *crtpCppmPacket =
(crtpCommanderCPPMEmuPacket_t*)&crtpPacket->data[1]; (crtpCommanderCPPMEmuPacket_t*)&crtpPacket->data[1];
@ -124,7 +124,7 @@ static void routeIncommingPacket(syslinkPacket_t* slp)
static void dataReceive(uint16_t c) static void dataReceive(uint16_t c)
{ {
counter++; counter++;
switch(rxState) { switch (rxState) {
case waitForFirstStart: case waitForFirstStart:
rxState = (c == SYSLINK_START_BYTE1) ? waitForSecondStart : waitForFirstStart; rxState = (c == SYSLINK_START_BYTE1) ? waitForSecondStart : waitForFirstStart;
break; break;
@ -205,7 +205,7 @@ bool targetCustomSerialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxR
{ {
rxRuntimeConfigPtr = rxRuntimeConfig; rxRuntimeConfigPtr = rxRuntimeConfig;
if(rxConfig->serialrx_provider != SERIALRX_TARGET_CUSTOM) if (rxConfig->serialrx_provider != SERIALRX_TARGET_CUSTOM)
{ {
return false; return false;
} }

View file

@ -63,7 +63,7 @@ void targetConfiguration(void)
featureSet(FEATURE_TELEMETRY); featureSet(FEATURE_TELEMETRY);
#endif #endif
parseRcChannels("TAER1234", rxConfigMutable()); parseRcChannels("TAER1234", rxConfigMutable());
pidProfilesMutable(0)->pid[PID_ROLL].P = 80; pidProfilesMutable(0)->pid[PID_ROLL].P = 80;
pidProfilesMutable(0)->pid[PID_ROLL].I = 37; pidProfilesMutable(0)->pid[PID_ROLL].I = 37;
pidProfilesMutable(0)->pid[PID_ROLL].D = 35; pidProfilesMutable(0)->pid[PID_ROLL].D = 35;
@ -72,7 +72,7 @@ void targetConfiguration(void)
pidProfilesMutable(0)->pid[PID_PITCH].D = 35; pidProfilesMutable(0)->pid[PID_PITCH].D = 35;
pidProfilesMutable(0)->pid[PID_YAW].P = 180; pidProfilesMutable(0)->pid[PID_YAW].P = 180;
pidProfilesMutable(0)->pid[PID_YAW].D = 45; pidProfilesMutable(0)->pid[PID_YAW].D = 45;
controlRateProfilesMutable(0)->rcRate8 = 100; controlRateProfilesMutable(0)->rcRate8 = 100;
controlRateProfilesMutable(0)->rcYawRate8 = 100; controlRateProfilesMutable(0)->rcYawRate8 = 100;
controlRateProfilesMutable(0)->rcExpo8 = 15; controlRateProfilesMutable(0)->rcExpo8 = 15;

View file

@ -17,7 +17,7 @@
#pragma once #pragma once
#define TARGET_BOARD_IDENTIFIER "FRF3" #define TARGET_BOARD_IDENTIFIER "FRF3"
#define TARGET_CONFIG #define TARGET_CONFIG
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
@ -111,7 +111,7 @@
#define SDCARD_SPI_CS_PIN SPI1_NSS_PIN #define SDCARD_SPI_CS_PIN SPI1_NSS_PIN
#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256
#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4
#define USE_ESC_SENSOR #define USE_ESC_SENSOR

View file

@ -91,7 +91,7 @@
#define USE_FLASH_M25P16 #define USE_FLASH_M25P16
#define M25P16_CS_PIN PB12 #define M25P16_CS_PIN PB12
#define M25P16_SPI_INSTANCE SPI2 #define M25P16_SPI_INSTANCE SPI2
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#define DEFAULT_FEATURES (FEATURE_OSD) #define DEFAULT_FEATURES (FEATURE_OSD)
@ -118,9 +118,9 @@
// Performance logging for SD card operations: // Performance logging for SD card operations:
// #define AFATFS_USE_INTROSPECTIVE_LOGGING // #define AFATFS_USE_INTROSPECTIVE_LOGGING
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
#define BARO #define BARO
#define USE_BARO_MS5611 #define USE_BARO_MS5611
@ -143,7 +143,7 @@
#define SOFTSERIAL1_RX_PIN PB0 #define SOFTSERIAL1_RX_PIN PB0
#define SOFTSERIAL1_TX_PIN PB1 #define SOFTSERIAL1_TX_PIN PB1
#define SONAR #define SONAR
#define SONAR_ECHO_PIN PB1 #define SONAR_ECHO_PIN PB1
#define SONAR_TRIGGER_PIN PB0 #define SONAR_TRIGGER_PIN PB0

View file

@ -78,7 +78,7 @@
#define MAX7456_SPI_CS_PIN PB12 #define MAX7456_SPI_CS_PIN PB12
#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) #define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2)
#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) #define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST)
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#define DEFAULT_FEATURES FEATURE_OSD #define DEFAULT_FEATURES FEATURE_OSD
@ -111,7 +111,7 @@
#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4
#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 #define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1
#define SDCARD_DMA_CHANNEL DMA_Channel_0 #define SDCARD_DMA_CHANNEL DMA_Channel_0
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
#endif #endif

View file

@ -29,7 +29,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM | TIM_USE_TRANSPONDER, TIMER_INPUT_ENABLED), // PPM IN DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM | TIM_USE_TRANSPONDER, TIMER_INPUT_ENABLED), // PPM IN
DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, TIMER_OUTPUT_ENABLED), // SS1Rx DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, TIMER_OUTPUT_ENABLED), // SS1Rx
DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, TIMER_OUTPUT_ENABLED), // SS1Tx DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, TIMER_OUTPUT_ENABLED), // SS1Tx
DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S1 DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S1
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S2 DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S2
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S3 DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S3

View file

@ -70,7 +70,7 @@
#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC
#define USE_ADC #define USE_ADC
#define CURRENT_METER_ADC_PIN PB1 #define CURRENT_METER_ADC_PIN PB1
#define VBAT_ADC_PIN PA0 #define VBAT_ADC_PIN PA0
#define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_SERIAL_4WAY_BLHELI_INTERFACE

View file

@ -110,6 +110,6 @@ void targetValidateConfiguration(void)
{ {
if (hardwareRevision < NAZE32_REV5 && accelerometerConfig()->acc_hardware == ACC_ADXL345) { if (hardwareRevision < NAZE32_REV5 && accelerometerConfig()->acc_hardware == ACC_ADXL345) {
accelerometerConfigMutable()->acc_hardware = ACC_NONE; accelerometerConfigMutable()->acc_hardware = ACC_NONE;
} }
} }
#endif #endif

View file

@ -79,7 +79,7 @@ void updateState(const fdm_packet* pkt) {
clock_gettime(CLOCK_MONOTONIC, &now_ts); clock_gettime(CLOCK_MONOTONIC, &now_ts);
const uint64_t realtime_now = micros64_real(); const uint64_t realtime_now = micros64_real();
if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout if (realtime_now > last_realtime + 500*1e3) { // 500ms timeout
last_timestamp = pkt->timestamp; last_timestamp = pkt->timestamp;
last_realtime = realtime_now; last_realtime = realtime_now;
sendMotorUpdate(); sendMotorUpdate();
@ -87,7 +87,7 @@ void updateState(const fdm_packet* pkt) {
} }
const double deltaSim = pkt->timestamp - last_timestamp; // in seconds const double deltaSim = pkt->timestamp - last_timestamp; // in seconds
if(deltaSim < 0) { // don't use old packet if (deltaSim < 0) { // don't use old packet
return; return;
} }
@ -141,7 +141,7 @@ void updateState(const fdm_packet* pkt) {
#endif #endif
if(deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz if (deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz
// simRate = simRate * 0.5 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.5; // simRate = simRate * 0.5 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.5;
struct timespec out_ts; struct timespec out_ts;
timeval_sub(&out_ts, &now_ts, &last_ts); timeval_sub(&out_ts, &now_ts, &last_ts);
@ -168,7 +168,7 @@ static void* udpThread(void* data) {
while (workerRunning) { while (workerRunning) {
n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100); n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100);
if(n == sizeof(fdm_packet)) { if (n == sizeof(fdm_packet)) {
// printf("[data]new fdm %d\n", n); // printf("[data]new fdm %d\n", n);
updateState(&fdmPkt); updateState(&fdmPkt);
} }
@ -215,7 +215,7 @@ void systemInit(void) {
} }
ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL); ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL);
if(ret != 0) { if (ret != 0) {
printf("Create tcpWorker error!\n"); printf("Create tcpWorker error!\n");
exit(1); exit(1);
} }
@ -227,7 +227,7 @@ void systemInit(void) {
printf("start UDP server...%d\n", ret); printf("start UDP server...%d\n", ret);
ret = pthread_create(&udpWorker, NULL, udpThread, NULL); ret = pthread_create(&udpWorker, NULL, udpThread, NULL);
if(ret != 0) { if (ret != 0) {
printf("Create udpWorker error!\n"); printf("Create udpWorker error!\n");
exit(1); exit(1);
} }
@ -266,7 +266,7 @@ void timerStart(void) {
void failureMode(failureMode_e mode) { void failureMode(failureMode_e mode) {
printf("[failureMode]!!! %d\n", mode); printf("[failureMode]!!! %d\n", mode);
while(1); while (1);
} }
void indicateFailure(failureMode_e mode, int repeatCount) void indicateFailure(failureMode_e mode, int repeatCount)
@ -440,7 +440,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) {
pwmPkt.motor_speed[2] = motorsPwm[3] / outScale; pwmPkt.motor_speed[2] = motorsPwm[3] / outScale;
// get one "fdm_packet" can only send one "servo_packet"!! // get one "fdm_packet" can only send one "servo_packet"!!
if(pthread_mutex_trylock(&updateLock) != 0) return; if (pthread_mutex_trylock(&updateLock) != 0) return;
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet)); udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
// printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]); // printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]);
} }
@ -483,7 +483,7 @@ void FLASH_Unlock(void) {
printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (long)(&__config_end - &__config_start)); printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (long)(&__config_end - &__config_start));
for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) { for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) {
int c = fgetc(eepromFd); int c = fgetc(eepromFd);
if(c == EOF) break; if (c == EOF) break;
eeprom[i] = (uint8_t)c; eeprom[i] = (uint8_t)c;
} }
} else { } else {

View file

@ -15,7 +15,7 @@
int udpInit(udpLink_t* link, const char* addr, int port, bool isServer) { int udpInit(udpLink_t* link, const char* addr, int port, bool isServer) {
int one = 1; int one = 1;
if((link->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) { if ((link->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
return -2; return -2;
} }
@ -28,13 +28,13 @@ int udpInit(udpLink_t* link, const char* addr, int port, bool isServer) {
link->si.sin_port = htons(port); link->si.sin_port = htons(port);
link->port = port; link->port = port;
if(addr == NULL) { if (addr == NULL) {
link->si.sin_addr.s_addr = htonl(INADDR_ANY); link->si.sin_addr.s_addr = htonl(INADDR_ANY);
}else{ }else{
link->si.sin_addr.s_addr = inet_addr(addr); link->si.sin_addr.s_addr = inet_addr(addr);
} }
if(isServer) { if (isServer) {
if (bind(link->fd, (const struct sockaddr *)&link->si, sizeof(link->si)) == -1) { if (bind(link->fd, (const struct sockaddr *)&link->si, sizeof(link->si)) == -1) {
return -1; return -1;
} }

View file

@ -83,7 +83,7 @@
/** /**
* @brief Adjust the value of External High Speed oscillator (HSE) used in your application. * @brief Adjust the value of External High Speed oscillator (HSE) used in your application.
* This value is used by the RCC HAL module to compute the system frequency * This value is used by the RCC HAL module to compute the system frequency
* (when HSE is used as system clock source, directly or through the PLL). * (when HSE is used as system clock source, directly or through the PLL).
*/ */
#if !defined (HSE_VALUE) #if !defined (HSE_VALUE)
#if defined(USE_STM3210C_EVAL) #if defined(USE_STM3210C_EVAL)
@ -114,22 +114,22 @@
#define LSE_VALUE ((uint32_t)32768) /*!< Value of the External oscillator in Hz*/ #define LSE_VALUE ((uint32_t)32768) /*!< Value of the External oscillator in Hz*/
#endif /* LSE_VALUE */ #endif /* LSE_VALUE */
#if !defined (LSE_STARTUP_TIMEOUT) #if !defined (LSE_STARTUP_TIMEOUT)
#define LSE_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for LSE start up, in ms */ #define LSE_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for LSE start up, in ms */
#endif /* HSE_STARTUP_TIMEOUT */ #endif /* HSE_STARTUP_TIMEOUT */
/* Tip: To avoid modifying this file each time you need to use different HSE, /* Tip: To avoid modifying this file each time you need to use different HSE,
=== you can define the HSE value in your toolchain compiler preprocessor. */ === you can define the HSE value in your toolchain compiler preprocessor. */
/* ########################### System Configuration ######################### */ /* ########################### System Configuration ######################### */
/** /**
* @brief This is the HAL system configuration section * @brief This is the HAL system configuration section
*/ */
#define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */ #define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */
#define TICK_INT_PRIORITY ((uint32_t)0x000F) /*!< tick interrupt priority */ #define TICK_INT_PRIORITY ((uint32_t)0x000F) /*!< tick interrupt priority */
#define USE_RTOS 0 #define USE_RTOS 0
#define PREFETCH_ENABLE 1 #define PREFETCH_ENABLE 1
/* ########################## Assert Selection ############################## */ /* ########################## Assert Selection ############################## */
@ -152,7 +152,7 @@
#define MAC_ADDR4 0 #define MAC_ADDR4 0
#define MAC_ADDR5 0 #define MAC_ADDR5 0
/* Definition of the Ethernet driver buffers size and count */ /* Definition of the Ethernet driver buffers size and count */
#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */ #define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */
#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */ #define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */
#define ETH_RXBUFNB ((uint32_t)8) /* 4 Rx buffers of size ETH_RX_BUF_SIZE */ #define ETH_RXBUFNB ((uint32_t)8) /* 4 Rx buffers of size ETH_RX_BUF_SIZE */
@ -189,7 +189,7 @@
#define PHY_AUTONEGO_COMPLETE ((uint16_t)0x0020) /*!< Auto-Negotiation process completed */ #define PHY_AUTONEGO_COMPLETE ((uint16_t)0x0020) /*!< Auto-Negotiation process completed */
#define PHY_LINKED_STATUS ((uint16_t)0x0004) /*!< Valid link established */ #define PHY_LINKED_STATUS ((uint16_t)0x0004) /*!< Valid link established */
#define PHY_JABBER_DETECTION ((uint16_t)0x0002) /*!< Jabber condition detected */ #define PHY_JABBER_DETECTION ((uint16_t)0x0002) /*!< Jabber condition detected */
/* Section 4: Extended PHY Registers */ /* Section 4: Extended PHY Registers */
#define PHY_SR ((uint16_t)0x10) /*!< PHY status register Offset */ #define PHY_SR ((uint16_t)0x10) /*!< PHY status register Offset */
@ -220,15 +220,15 @@
#ifdef HAL_GPIO_MODULE_ENABLED #ifdef HAL_GPIO_MODULE_ENABLED
#include "stm32f1xx_hal_gpio.h" #include "stm32f1xx_hal_gpio.h"
#endif /* HAL_GPIO_MODULE_ENABLED */ #endif /* HAL_GPIO_MODULE_ENABLED */
#ifdef HAL_DMA_MODULE_ENABLED #ifdef HAL_DMA_MODULE_ENABLED
#include "stm32f1xx_hal_dma.h" #include "stm32f1xx_hal_dma.h"
#endif /* HAL_DMA_MODULE_ENABLED */ #endif /* HAL_DMA_MODULE_ENABLED */
#ifdef HAL_ETH_MODULE_ENABLED #ifdef HAL_ETH_MODULE_ENABLED
#include "stm32f1xx_hal_eth.h" #include "stm32f1xx_hal_eth.h"
#endif /* HAL_ETH_MODULE_ENABLED */ #endif /* HAL_ETH_MODULE_ENABLED */
#ifdef HAL_CAN_MODULE_ENABLED #ifdef HAL_CAN_MODULE_ENABLED
#include "stm32f1xx_hal_can.h" #include "stm32f1xx_hal_can.h"
#endif /* HAL_CAN_MODULE_ENABLED */ #endif /* HAL_CAN_MODULE_ENABLED */
@ -291,11 +291,11 @@
#ifdef HAL_SD_MODULE_ENABLED #ifdef HAL_SD_MODULE_ENABLED
#include "stm32f1xx_hal_sd.h" #include "stm32f1xx_hal_sd.h"
#endif /* HAL_SD_MODULE_ENABLED */ #endif /* HAL_SD_MODULE_ENABLED */
#ifdef HAL_NAND_MODULE_ENABLED #ifdef HAL_NAND_MODULE_ENABLED
#include "stm32f1xx_hal_nand.h" #include "stm32f1xx_hal_nand.h"
#endif /* HAL_NAND_MODULE_ENABLED */ #endif /* HAL_NAND_MODULE_ENABLED */
#ifdef HAL_SPI_MODULE_ENABLED #ifdef HAL_SPI_MODULE_ENABLED
#include "stm32f1xx_hal_spi.h" #include "stm32f1xx_hal_spi.h"
@ -332,8 +332,8 @@
#ifdef HAL_HCD_MODULE_ENABLED #ifdef HAL_HCD_MODULE_ENABLED
#include "stm32f1xx_hal_hcd.h" #include "stm32f1xx_hal_hcd.h"
#endif /* HAL_HCD_MODULE_ENABLED */ #endif /* HAL_HCD_MODULE_ENABLED */
/* Exported macro ------------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/
#ifdef USE_FULL_ASSERT #ifdef USE_FULL_ASSERT

View file

@ -86,7 +86,7 @@
/** /**
* @brief Adjust the value of External High Speed oscillator (HSE) used in your application. * @brief Adjust the value of External High Speed oscillator (HSE) used in your application.
* This value is used by the RCC HAL module to compute the system frequency * This value is used by the RCC HAL module to compute the system frequency
* (when HSE is used as system clock source, directly or through the PLL). * (when HSE is used as system clock source, directly or through the PLL).
*/ */
#if !defined (HSE_VALUE) #if !defined (HSE_VALUE)
#define HSE_VALUE ((uint32_t)8000000) /*!< Value of the External oscillator in Hz */ #define HSE_VALUE ((uint32_t)8000000) /*!< Value of the External oscillator in Hz */
@ -115,13 +115,13 @@
*/ */
#if !defined (HSI_STARTUP_TIMEOUT) #if !defined (HSI_STARTUP_TIMEOUT)
#define HSI_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for HSI start up */ #define HSI_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for HSI start up */
#endif /* HSI_STARTUP_TIMEOUT */ #endif /* HSI_STARTUP_TIMEOUT */
/** /**
* @brief Internal Low Speed oscillator (LSI) value. * @brief Internal Low Speed oscillator (LSI) value.
*/ */
#if !defined (LSI_VALUE) #if !defined (LSI_VALUE)
#define LSI_VALUE ((uint32_t)40000) #define LSI_VALUE ((uint32_t)40000)
#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz #endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz
The real value may vary depending on the variations The real value may vary depending on the variations
in voltage and temperature. */ in voltage and temperature. */
@ -130,7 +130,7 @@
*/ */
#if !defined (LSE_VALUE) #if !defined (LSE_VALUE)
#define LSE_VALUE ((uint32_t)32768) /*!< Value of the External Low Speed oscillator in Hz */ #define LSE_VALUE ((uint32_t)32768) /*!< Value of the External Low Speed oscillator in Hz */
#endif /* LSE_VALUE */ #endif /* LSE_VALUE */
/** /**
* @brief Time out for LSE start up value in ms. * @brief Time out for LSE start up value in ms.
@ -156,7 +156,7 @@
/* ########################### System Configuration ######################### */ /* ########################### System Configuration ######################### */
/** /**
* @brief This is the HAL system configuration section * @brief This is the HAL system configuration section
*/ */
#define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */ #define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */
#define TICK_INT_PRIORITY ((uint32_t)(1<<__NVIC_PRIO_BITS) - 1) /*!< tick interrupt priority (lowest by default) */ #define TICK_INT_PRIORITY ((uint32_t)(1<<__NVIC_PRIO_BITS) - 1) /*!< tick interrupt priority (lowest by default) */
#define USE_RTOS 0 #define USE_RTOS 0
@ -187,7 +187,7 @@
#ifdef HAL_DMA_MODULE_ENABLED #ifdef HAL_DMA_MODULE_ENABLED
#include "stm32f3xx_hal_dma.h" #include "stm32f3xx_hal_dma.h"
#endif /* HAL_DMA_MODULE_ENABLED */ #endif /* HAL_DMA_MODULE_ENABLED */
#ifdef HAL_CORTEX_MODULE_ENABLED #ifdef HAL_CORTEX_MODULE_ENABLED
#include "stm32f3xx_hal_cortex.h" #include "stm32f3xx_hal_cortex.h"
#endif /* HAL_CORTEX_MODULE_ENABLED */ #endif /* HAL_CORTEX_MODULE_ENABLED */
@ -235,7 +235,7 @@
#ifdef HAL_PCCARD_MODULE_ENABLED #ifdef HAL_PCCARD_MODULE_ENABLED
#include "stm32f3xx_hal_pccard.h" #include "stm32f3xx_hal_pccard.h"
#endif /* HAL_PCCARD_MODULE_ENABLED */ #endif /* HAL_PCCARD_MODULE_ENABLED */
#ifdef HAL_HRTIM_MODULE_ENABLED #ifdef HAL_HRTIM_MODULE_ENABLED
#include "stm32f3xx_hal_hrtim.h" #include "stm32f3xx_hal_hrtim.h"
#endif /* HAL_HRTIM_MODULE_ENABLED */ #endif /* HAL_HRTIM_MODULE_ENABLED */
@ -323,8 +323,8 @@
void assert_failed(uint8_t* file, uint32_t line); void assert_failed(uint8_t* file, uint32_t line);
#else #else
#define assert_param(expr) ((void)0U) #define assert_param(expr) ((void)0U)
#endif /* USE_FULL_ASSERT */ #endif /* USE_FULL_ASSERT */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

View file

@ -47,7 +47,7 @@
/** /**
* @brief This is the list of modules to be used in the HAL driver * @brief This is the list of modules to be used in the HAL driver
*/ */
#define HAL_MODULE_ENABLED #define HAL_MODULE_ENABLED
#define HAL_ADC_MODULE_ENABLED #define HAL_ADC_MODULE_ENABLED
//#define HAL_CAN_MODULE_ENABLED //#define HAL_CAN_MODULE_ENABLED
//#define HAL_CRC_MODULE_ENABLED //#define HAL_CRC_MODULE_ENABLED
@ -97,7 +97,7 @@
/** /**
* @brief Adjust the value of External High Speed oscillator (HSE) used in your application. * @brief Adjust the value of External High Speed oscillator (HSE) used in your application.
* This value is used by the RCC HAL module to compute the system frequency * This value is used by the RCC HAL module to compute the system frequency
* (when HSE is used as system clock source, directly or through the PLL). * (when HSE is used as system clock source, directly or through the PLL).
*/ */
#if !defined (HSE_VALUE) #if !defined (HSE_VALUE)
#define HSE_VALUE ((uint32_t)8000000U) /*!< Value of the External oscillator in Hz */ #define HSE_VALUE ((uint32_t)8000000U) /*!< Value of the External oscillator in Hz */
@ -150,7 +150,7 @@
/* ########################### System Configuration ######################### */ /* ########################### System Configuration ######################### */
/** /**
* @brief This is the HAL system configuration section * @brief This is the HAL system configuration section
*/ */
#define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */ #define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */
#define TICK_INT_PRIORITY ((uint32_t)0x0FU) /*!< tick interrupt priority */ #define TICK_INT_PRIORITY ((uint32_t)0x0FU) /*!< tick interrupt priority */
#define USE_RTOS 0U #define USE_RTOS 0U
@ -177,7 +177,7 @@
#define MAC_ADDR4 0U #define MAC_ADDR4 0U
#define MAC_ADDR5 0U #define MAC_ADDR5 0U
/* Definition of the Ethernet driver buffers size and count */ /* Definition of the Ethernet driver buffers size and count */
#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */ #define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */
#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */ #define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */
#define ETH_RXBUFNB ((uint32_t)4U) /* 4 Rx buffers of size ETH_RX_BUF_SIZE */ #define ETH_RXBUFNB ((uint32_t)4U) /* 4 Rx buffers of size ETH_RX_BUF_SIZE */
@ -214,7 +214,7 @@
#define PHY_AUTONEGO_COMPLETE ((uint16_t)0x0020U) /*!< Auto-Negotiation process completed */ #define PHY_AUTONEGO_COMPLETE ((uint16_t)0x0020U) /*!< Auto-Negotiation process completed */
#define PHY_LINKED_STATUS ((uint16_t)0x0004U) /*!< Valid link established */ #define PHY_LINKED_STATUS ((uint16_t)0x0004U) /*!< Valid link established */
#define PHY_JABBER_DETECTION ((uint16_t)0x0002U) /*!< Jabber condition detected */ #define PHY_JABBER_DETECTION ((uint16_t)0x0002U) /*!< Jabber condition detected */
/* Section 4: Extended PHY Registers */ /* Section 4: Extended PHY Registers */
#define PHY_SR ((uint16_t)0x0010U) /*!< PHY status register Offset */ #define PHY_SR ((uint16_t)0x0010U) /*!< PHY status register Offset */
@ -256,7 +256,7 @@
#ifdef HAL_DMA_MODULE_ENABLED #ifdef HAL_DMA_MODULE_ENABLED
#include "stm32f4xx_hal_dma.h" #include "stm32f4xx_hal_dma.h"
#endif /* HAL_DMA_MODULE_ENABLED */ #endif /* HAL_DMA_MODULE_ENABLED */
#ifdef HAL_CORTEX_MODULE_ENABLED #ifdef HAL_CORTEX_MODULE_ENABLED
#include "stm32f4xx_hal_cortex.h" #include "stm32f4xx_hal_cortex.h"
#endif /* HAL_CORTEX_MODULE_ENABLED */ #endif /* HAL_CORTEX_MODULE_ENABLED */
@ -312,7 +312,7 @@
#ifdef HAL_PCCARD_MODULE_ENABLED #ifdef HAL_PCCARD_MODULE_ENABLED
#include "stm32f4xx_hal_pccard.h" #include "stm32f4xx_hal_pccard.h"
#endif /* HAL_PCCARD_MODULE_ENABLED */ #endif /* HAL_PCCARD_MODULE_ENABLED */
#ifdef HAL_SDRAM_MODULE_ENABLED #ifdef HAL_SDRAM_MODULE_ENABLED
#include "stm32f4xx_hal_sdram.h" #include "stm32f4xx_hal_sdram.h"
#endif /* HAL_SDRAM_MODULE_ENABLED */ #endif /* HAL_SDRAM_MODULE_ENABLED */
@ -392,7 +392,7 @@
#ifdef HAL_HCD_MODULE_ENABLED #ifdef HAL_HCD_MODULE_ENABLED
#include "stm32f4xx_hal_hcd.h" #include "stm32f4xx_hal_hcd.h"
#endif /* HAL_HCD_MODULE_ENABLED */ #endif /* HAL_HCD_MODULE_ENABLED */
#ifdef HAL_DSI_MODULE_ENABLED #ifdef HAL_DSI_MODULE_ENABLED
#include "stm32f4xx_hal_dsi.h" #include "stm32f4xx_hal_dsi.h"
#endif /* HAL_DSI_MODULE_ENABLED */ #endif /* HAL_DSI_MODULE_ENABLED */

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

@ -642,7 +642,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)
{ {
@ -700,18 +700,18 @@ void SetSysClock(void)
RCC->CR |= RCC_CR_PLLON; RCC->CR |= RCC_CR_PLLON;
/* Wait till the main PLL is ready */ /* Wait till the main PLL is ready */
while((RCC->CR & RCC_CR_PLLRDY) == 0) while ((RCC->CR & RCC_CR_PLLRDY) == 0)
{ {
} }
#if defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) || defined(STM32F469_479xx) #if defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) || defined(STM32F469_479xx)
/* Enable the Over-drive to extend the clock frequency to 180 Mhz */ /* Enable the Over-drive to extend the clock frequency to 180 Mhz */
PWR->CR |= PWR_CR_ODEN; PWR->CR |= PWR_CR_ODEN;
while((PWR->CSR & PWR_CSR_ODRDY) == 0) while ((PWR->CSR & PWR_CSR_ODRDY) == 0)
{ {
} }
PWR->CR |= PWR_CR_ODSWEN; PWR->CR |= PWR_CR_ODSWEN;
while((PWR->CSR & PWR_CSR_ODSWRDY) == 0) while ((PWR->CSR & PWR_CSR_ODSWRDY) == 0)
{ {
} }
#endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ #endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */
@ -1018,7 +1018,7 @@ void SystemInit_ExtMemCtl(void)
/* Clock enable command */ /* Clock enable command */
FMC_Bank5_6->SDCMR = 0x00000011; FMC_Bank5_6->SDCMR = 0x00000011;
tmpreg = FMC_Bank5_6->SDSR & 0x00000020; tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
while((tmpreg != 0) & (timeout-- > 0)) while ((tmpreg != 0) & (timeout-- > 0))
{ {
tmpreg = FMC_Bank5_6->SDSR & 0x00000020; tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
} }
@ -1029,7 +1029,7 @@ void SystemInit_ExtMemCtl(void)
/* PALL command */ /* PALL command */
FMC_Bank5_6->SDCMR = 0x00000012; FMC_Bank5_6->SDCMR = 0x00000012;
timeout = 0xFFFF; timeout = 0xFFFF;
while((tmpreg != 0) & (timeout-- > 0)) while ((tmpreg != 0) & (timeout-- > 0))
{ {
tmpreg = FMC_Bank5_6->SDSR & 0x00000020; tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
} }
@ -1037,7 +1037,7 @@ void SystemInit_ExtMemCtl(void)
/* Auto refresh command */ /* Auto refresh command */
FMC_Bank5_6->SDCMR = 0x00000073; FMC_Bank5_6->SDCMR = 0x00000073;
timeout = 0xFFFF; timeout = 0xFFFF;
while((tmpreg != 0) & (timeout-- > 0)) while ((tmpreg != 0) & (timeout-- > 0))
{ {
tmpreg = FMC_Bank5_6->SDSR & 0x00000020; tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
} }
@ -1045,7 +1045,7 @@ void SystemInit_ExtMemCtl(void)
/* MRD register program */ /* MRD register program */
FMC_Bank5_6->SDCMR = 0x00046014; FMC_Bank5_6->SDCMR = 0x00046014;
timeout = 0xFFFF; timeout = 0xFFFF;
while((tmpreg != 0) & (timeout-- > 0)) while ((tmpreg != 0) & (timeout-- > 0))
{ {
tmpreg = FMC_Bank5_6->SDSR & 0x00000020; tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
} }

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

@ -231,10 +231,10 @@ static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage)
{ {
batteryState_e batteryState; batteryState_e batteryState;
if (shouldTriggerBatteryAlarmNow()){ if (shouldTriggerBatteryAlarmNow()) {
lastHottAlarmSoundTime = millis(); lastHottAlarmSoundTime = millis();
batteryState = getBatteryState(); batteryState = getBatteryState();
if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL){ if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) {
hottEAMMessage->warning_beeps = 0x10; hottEAMMessage->warning_beeps = 0x10;
hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_BATTERY_1; hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_BATTERY_1;
} }
@ -282,7 +282,7 @@ static inline void hottEAMUpdateClimbrate(HOTT_EAM_MSG_t *hottEAMMessage)
int32_t vario = getEstimatedVario(); int32_t vario = getEstimatedVario();
hottEAMMessage->climbrate_L = (30000 + vario) & 0x00FF; hottEAMMessage->climbrate_L = (30000 + vario) & 0x00FF;
hottEAMMessage->climbrate_H = (30000 + vario) >> 8; hottEAMMessage->climbrate_H = (30000 + vario) >> 8;
hottEAMMessage->climbrate3s = 120 + (vario / 100); hottEAMMessage->climbrate3s = 120 + (vario / 100);
} }
void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage) void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage)
@ -401,7 +401,7 @@ void configureHoTTTelemetryPort(void)
static void hottSendResponse(uint8_t *buffer, int length) static void hottSendResponse(uint8_t *buffer, int length)
{ {
if(hottIsSending) { if (hottIsSending) {
return; return;
} }
@ -514,7 +514,7 @@ static void hottSendTelemetryData(void) {
hottReconfigurePort(); hottReconfigurePort();
--hottMsgRemainingBytesToSendCount; --hottMsgRemainingBytesToSendCount;
if(hottMsgRemainingBytesToSendCount == 0) { if (hottMsgRemainingBytesToSendCount == 0) {
hottSerialWrite(hottMsgCrc++); hottSerialWrite(hottMsgCrc++);
return; return;
} }
@ -572,7 +572,7 @@ void handleHoTTTelemetry(timeUs_t currentTimeUs)
return; return;
if (hottIsSending) { if (hottIsSending) {
if(currentTimeUs - serialTimer < HOTT_TX_DELAY_US) { if (currentTimeUs - serialTimer < HOTT_TX_DELAY_US) {
return; return;
} }
} }

View file

@ -122,7 +122,7 @@ static ibusAddress_t getAddress(const uint8_t *ibusPacket)
static uint8_t dispatchMeasurementReply(ibusAddress_t address) static uint8_t dispatchMeasurementReply(ibusAddress_t address)
{ {
int value; int value;
switch (sensorAddressTypeLookup[address - ibusBaseAddress]) { switch (sensorAddressTypeLookup[address - ibusBaseAddress]) {
case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE: case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE:
value = getBatteryVoltage() * 10; value = getBatteryVoltage() * 10;

View file

@ -436,7 +436,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

@ -242,7 +242,7 @@ static void smartPortDataReceive(uint16_t c)
rxBuffer[smartPortRxBytes++] = c; rxBuffer[smartPortRxBytes++] = c;
if(smartPortRxBytes == SMARTPORT_FRAME_SIZE) { if (smartPortRxBytes == SMARTPORT_FRAME_SIZE) {
if (c == (0xFF - checksum)) { if (c == (0xFF - checksum)) {
smartPortFrameReceived = true; smartPortFrameReceived = true;
} }
@ -280,7 +280,7 @@ static void smartPortSendPackageEx(uint8_t frameId, uint8_t* data)
{ {
uint16_t crc = 0; uint16_t crc = 0;
smartPortSendByte(frameId, &crc); smartPortSendByte(frameId, &crc);
for(unsigned i = 0; i < SMARTPORT_PAYLOAD_SIZE; i++) { for (unsigned i = 0; i < SMARTPORT_PAYLOAD_SIZE; i++) {
smartPortSendByte(*data++, &crc); smartPortSendByte(*data++, &crc);
} }
smartPortSendByte(0xFF - (uint8_t)crc, NULL); smartPortSendByte(0xFF - (uint8_t)crc, NULL);
@ -559,11 +559,11 @@ void handleSmartPortTelemetry(void)
smartPortDataReceive(c); smartPortDataReceive(c);
} }
if(smartPortFrameReceived) { if (smartPortFrameReceived) {
smartPortFrameReceived = false; smartPortFrameReceived = false;
// do not check the physical ID here again // do not check the physical ID here again
// unless we start receiving other sensors' packets // unless we start receiving other sensors' packets
if(smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) { if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) {
// Pass only the payload: skip sensorId & frameId // Pass only the payload: skip sensorId & frameId
handleSmartPortMspFrame(&smartPortRxBuffer); handleSmartPortMspFrame(&smartPortRxBuffer);
@ -577,7 +577,7 @@ void handleSmartPortTelemetry(void)
return; return;
} }
if(smartPortMspReplyPending) { if (smartPortMspReplyPending) {
smartPortMspReplyPending = smartPortSendMspReply(); smartPortMspReplyPending = smartPortSendMspReply();
smartPortHasRequest = 0; smartPortHasRequest = 0;
return; return;
@ -596,7 +596,7 @@ void handleSmartPortTelemetry(void)
static uint8_t t1Cnt = 0; static uint8_t t1Cnt = 0;
static uint8_t t2Cnt = 0; static uint8_t t2Cnt = 0;
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)) {
@ -742,7 +742,7 @@ void handleSmartPortTelemetry(void)
} else if (feature(FEATURE_GPS)) { } else if (feature(FEATURE_GPS)) {
smartPortSendPackage(id, 0); smartPortSendPackage(id, 0);
smartPortHasRequest = 0; smartPortHasRequest = 0;
} else if (telemetryConfig()->pidValuesAsTelemetry){ } else if (telemetryConfig()->pidValuesAsTelemetry) {
switch (t2Cnt) { switch (t2Cnt) {
case 0: case 0:
tmp2 = currentPidProfile->pid[PID_ROLL].P; tmp2 = currentPidProfile->pid[PID_ROLL].P;

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);