diff --git a/src/board.h b/src/board.h index fa2962220c..87765d9ccc 100755 --- a/src/board.h +++ b/src/board.h @@ -142,8 +142,7 @@ enum { TEMP_UPDATED = 1 << 3 }; -typedef struct sensor_data_t -{ +typedef struct sensor_data_t { int16_t gyro[3]; int16_t acc[3]; int16_t mag[3]; @@ -151,24 +150,22 @@ typedef struct sensor_data_t int updated; } sensor_data_t; -typedef void (* sensorInitFuncPtr)(sensor_align_e align); // sensor init prototype -typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype -typedef void (* baroOpFuncPtr)(void); // baro start operation -typedef void (* baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature) -typedef void (* serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app -typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data -typedef void (* pidControllerFuncPtr)(void); // pid controller function prototype +typedef void (*sensorInitFuncPtr)(sensor_align_e align); // sensor init prototype +typedef void (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype +typedef void (*baroOpFuncPtr)(void); // baro start operation +typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature) +typedef void (*serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app +typedef uint16_t (*rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data +typedef void (*pidControllerFuncPtr)(void); // pid controller function prototype -typedef struct sensor_t -{ +typedef struct sensor_t { sensorInitFuncPtr init; // initialize function sensorReadFuncPtr read; // read 3 axis data function sensorReadFuncPtr temperature; // read temperature if available float scale; // scalefactor (currently used for gyro only, todo for accel) } sensor_t; -typedef struct baro_t -{ +typedef struct baro_t { uint16_t ut_delay; uint16_t up_delay; baroOpFuncPtr start_ut; diff --git a/src/buzzer.c b/src/buzzer.c index 56bbc40090..303834cd5a 100644 --- a/src/buzzer.c +++ b/src/buzzer.c @@ -78,7 +78,7 @@ void beep_code(char first, char second, char third, char pause) patternChar[1] = second; patternChar[2] = third; patternChar[3] = pause; - switch(patternChar[icnt]) { + switch (patternChar[icnt]) { case 'M': Duration = 100; break; @@ -121,7 +121,7 @@ static void beep(uint16_t pulse) buzzerIsOn = 0; BEEP_OFF; buzzerLastToggleTime = millis(); - if (toggleBeep >0) + if (toggleBeep > 0) toggleBeep--; beepDone = 1; } diff --git a/src/cli.c b/src/cli.c index 829117a83e..fbd20e19b6 100644 --- a/src/cli.c +++ b/src/cli.c @@ -297,7 +297,7 @@ static float _atof(const char *p) float sign, value, scale; // Skip leading white space, if any. - while (white_space(*p) ) { + while (white_space(*p)) { p += 1; } @@ -352,7 +352,7 @@ static float _atof(const char *p) expon = expon * 10 + (*p - '0'); p += 1; } - if (expon > 308) + if (expon > 308) expon = 308; // Calculate scaling factor. @@ -381,7 +381,7 @@ static char *ftoa(float x, char *floatString) else x -= 0.0005f; - value = (int32_t) (x * 1000.0f); // Convert float * 1000 to an integer + value = (int32_t)(x * 1000.0f); // Convert float * 1000 to an integer itoa(abs(value), intString1, 10); // Create string from abs of integer value diff --git a/src/config.c b/src/config.c index 29a21d7901..2ff77ed6c2 100755 --- a/src/config.c +++ b/src/config.c @@ -65,14 +65,14 @@ void readEEPROM(void) // Copy current profile if (mcfg.current_profile > 2) // sanity check mcfg.current_profile = 0; - memcpy(&cfg, &mcfg.profile[mcfg.current_profile], sizeof(config_t)); + memcpy(&cfg, &mcfg.profile[mcfg.current_profile], sizeof(config_t)); } void activateConfig(void) { uint8_t i; for (i = 0; i < PITCH_LOOKUP_LENGTH; i++) - lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 2500; + lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t)cfg.rcRate8 / 2500; for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { int16_t tmp = 10 * i - cfg.thrMid8; @@ -81,8 +81,8 @@ void activateConfig(void) y = 100 - cfg.thrMid8; if (tmp < 0) y = cfg.thrMid8; - lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t) cfg.thrExpo8 * (tmp * tmp) / (y * y)) / 10; - lookupThrottleRC[i] = mcfg.minthrottle + (int32_t) (mcfg.maxthrottle - mcfg.minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] + lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t)cfg.thrExpo8 * (tmp * tmp) / (y * y)) / 10; + lookupThrottleRC[i] = mcfg.minthrottle + (int32_t)(mcfg.maxthrottle - mcfg.minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] } setPIDController(cfg.pidController); @@ -128,7 +128,7 @@ retry: if (FLASH_ErasePage(FLASH_WRITE_ADDR) == FLASH_COMPLETE) { for (i = 0; i < sizeof(master_t); i += 4) { - status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *) ((char *)&mcfg + i)); + status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *)((char *)&mcfg + i)); if (status != FLASH_COMPLETE) { FLASH_Lock(); tries++; diff --git a/src/drv_adxl345.c b/src/drv_adxl345.c index a8890e2e9a..d0c9827ded 100755 --- a/src/drv_adxl345.c +++ b/src/drv_adxl345.c @@ -60,7 +60,7 @@ bool adxl345Detect(drv_adxl345_config_t *init, sensor_t *acc) static void adxl345Init(sensor_align_e align) { - if (useFifo) { + if (useFifo) { uint8_t fifoDepth = 16; i2cWrite(ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS); i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G); diff --git a/src/drv_bmp085.c b/src/drv_bmp085.c index 2fd7a45a22..ad761e91fd 100755 --- a/src/drv_bmp085.c +++ b/src/drv_bmp085.c @@ -30,7 +30,7 @@ typedef struct { int16_t md; } bmp085_smd500_calibration_param_t; -typedef struct { +typedef struct { bmp085_smd500_calibration_param_t cal_param; uint8_t mode; uint8_t chip_id, ml_version, al_version; @@ -189,9 +189,9 @@ static int32_t bmp085_get_pressure(uint32_t up) // *****calculate B4************ x1 = (bmp085.cal_param.ac3 * b6) >> 13; - x2 = (bmp085.cal_param.b1 * ((b6 * b6) >> 12) ) >> 16; + x2 = (bmp085.cal_param.b1 * ((b6 * b6) >> 12)) >> 16; x3 = ((x1 + x2) + 2) >> 2; - b4 = (bmp085.cal_param.ac4 * (uint32_t) (x3 + 32768)) >> 15; + b4 = (bmp085.cal_param.ac4 * (uint32_t)(x3 + 32768)) >> 15; b7 = ((uint32_t)(up - b3) * (50000 >> bmp085.oversampling_setting)); if (b7 < 0x80000000) { @@ -249,7 +249,7 @@ static void bmp085_get_up(void) convOverrun++; i2cRead(BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data); - bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - bmp085.oversampling_setting); + bmp085_up = (((uint32_t)data[0] << 16) | ((uint32_t)data[1] << 8) | (uint32_t)data[2]) >> (8 - bmp085.oversampling_setting); } static void bmp085_calculate(int32_t *pressure, int32_t *temperature) diff --git a/src/drv_gpio.c b/src/drv_gpio.c index 4214b85951..7acc631be3 100644 --- a/src/drv_gpio.c +++ b/src/drv_gpio.c @@ -71,7 +71,7 @@ void gpioPinRemapConfig(uint32_t remap, bool enable) } if (enable) - tmpreg |= (tmp << ((remap >> 0x15)*0x10)); + tmpreg |= (tmp << ((remap >> 0x15) * 0x10)); if ((remap & 0x80000000) == 0x80000000) AFIO->MAPR2 = tmpreg; diff --git a/src/drv_gpio.h b/src/drv_gpio.h index cc0336f564..4e52420934 100644 --- a/src/drv_gpio.h +++ b/src/drv_gpio.h @@ -1,7 +1,6 @@ #pragma once -typedef enum -{ +typedef enum { Mode_AIN = 0x0, Mode_IN_FLOATING = 0x04, Mode_IPD = 0x28, @@ -12,15 +11,13 @@ typedef enum Mode_AF_PP = 0x18 } GPIO_Mode; -typedef enum -{ +typedef enum { Speed_10MHz = 1, Speed_2MHz, Speed_50MHz } GPIO_Speed; -typedef enum -{ +typedef enum { Pin_0 = 0x0001, Pin_1 = 0x0002, Pin_2 = 0x0004, @@ -40,8 +37,7 @@ typedef enum Pin_All = 0xFFFF } GPIO_Pin; -typedef struct -{ +typedef struct { uint16_t pin; GPIO_Mode mode; GPIO_Speed speed; diff --git a/src/drv_hcsr04.c b/src/drv_hcsr04.c index 1bc426f529..73a1c296f0 100644 --- a/src/drv_hcsr04.c +++ b/src/drv_hcsr04.c @@ -61,7 +61,7 @@ void hcsr04_init(sonar_config_t config) // enable AFIO for EXTI support - already done is drv_system.c // RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph, ENABLE); - switch(config) { + switch (config) { case sonar_pwm56: trigger_pin = Pin_8; // PWM5 (PB8) - 5v tolerant echo_pin = Pin_9; // PWM6 (PB9) - 5v tolerant diff --git a/src/drv_ms5611.c b/src/drv_ms5611.c index 42cd3bb145..02928ed0ba 100644 --- a/src/drv_ms5611.c +++ b/src/drv_ms5611.c @@ -156,11 +156,11 @@ static void ms5611_calculate(int32_t *pressure, int32_t *temperature) { uint32_t press; int64_t temp; - int64_t delt; + int64_t delt; int32_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256); int64_t off = ((int64_t)ms5611_c[2] << 16) + (((int64_t)ms5611_c[4] * dT) >> 7); int64_t sens = ((int64_t)ms5611_c[1] << 15) + (((int64_t)ms5611_c[3] * dT) >> 8); - temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23); + temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23); if (temp < 2000) { // temperature lower than 20degC delt = temp - 2000; @@ -170,11 +170,11 @@ static void ms5611_calculate(int32_t *pressure, int32_t *temperature) if (temp < -1500) { // temperature lower than -15degC delt = temp + 1500; delt = delt * delt; - off -= 7 * delt; + off -= 7 * delt; sens -= (11 * delt) >> 1; } } - press = ((((int64_t)ms5611_up * sens ) >> 21) - off) >> 15; + press = ((((int64_t)ms5611_up * sens) >> 21) - off) >> 15; if (pressure) *pressure = press; diff --git a/src/drv_pwm.c b/src/drv_pwm.c index 1773e5de18..c9fef7f04e 100755 --- a/src/drv_pwm.c +++ b/src/drv_pwm.c @@ -51,7 +51,7 @@ enum { TYPE_S = 0x80 }; -typedef void (* pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors +typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors static pwmPortData_t pwmPorts[MAX_PORTS]; static uint16_t captures[MAX_INPUTS]; @@ -60,7 +60,7 @@ static pwmPortData_t *servos[MAX_SERVOS]; static pwmWriteFuncPtr pwmWritePtr = NULL; static uint8_t numMotors = 0; static uint8_t numServos = 0; -static uint8_t numInputs = 0; +static uint8_t numInputs = 0; static uint16_t failsafeThreshold = 985; // external vars (ugh) extern int16_t failsafeCnt; @@ -143,7 +143,7 @@ static const uint8_t * const hardwareMaps[] = { static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value) { - TIM_OCInitTypeDef TIM_OCInitStructure; + TIM_OCInitTypeDef TIM_OCInitStructure; TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; @@ -175,7 +175,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value) void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) { - TIM_ICInitTypeDef TIM_ICInitStructure; + TIM_ICInitTypeDef TIM_ICInitStructure; TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_Channel = channel; @@ -245,7 +245,7 @@ static pwmPortData_t *pwmInConfig(uint8_t port, timerCCCallbackPtr callback, uin static void failsafeCheck(uint8_t channel, uint16_t pulse) { static uint8_t goodPulses; - + if (channel < 4 && pulse > failsafeThreshold) goodPulses |= (1 << channel); // if signal is valid - mark channel as OK if (goodPulses == 0x0F) { // If first four chanells have good pulses, clear FailSafe counter diff --git a/src/drv_pwm_fy90q.c b/src/drv_pwm_fy90q.c index a4448ff183..b1236f9865 100644 --- a/src/drv_pwm_fy90q.c +++ b/src/drv_pwm_fy90q.c @@ -48,7 +48,7 @@ static struct PWM_State { uint16_t capture; } Inputs[8] = { { 0, } }; -static TIM_ICInitTypeDef TIM_ICInitStructure = { 0, }; +static TIM_ICInitTypeDef TIM_ICInitStructure = { 0, }; static bool usePPMFlag = false; static uint8_t numOutputChannels = 0; @@ -165,11 +165,11 @@ static void pwmIRQHandler(TIM_TypeDef *tim) static void pwmInitializeInput(bool usePPM) { GPIO_InitTypeDef GPIO_InitStructure = { 0, }; - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, }; + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, }; NVIC_InitTypeDef NVIC_InitStructure = { 0, }; uint8_t i; - // Input pins + // Input pins if (usePPM) { // Configure TIM2_CH1 for PPM input GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0; @@ -253,7 +253,7 @@ static void pwmInitializeInput(bool usePPM) bool pwmInit(drv_pwm_config_t *init) { GPIO_InitTypeDef GPIO_InitStructure = { 0, }; - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, }; + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, }; TIM_OCInitTypeDef TIM_OCInitStructure = { 0, }; uint8_t i; diff --git a/src/drv_serial.h b/src/drv_serial.h index 22adee4ce2..7799690e2f 100644 --- a/src/drv_serial.h +++ b/src/drv_serial.h @@ -1,16 +1,16 @@ #pragma once typedef enum portMode_t { - MODE_RX = 1 << 0, + MODE_RX = 1 << 0, MODE_TX = 1 << 1, MODE_RXTX = MODE_RX | MODE_TX, MODE_SBUS = 1 << 2, } portMode_t; typedef struct serialPort { - + const struct serialPortVTable *vTable; - + portMode_t mode; uint32_t baudRate; @@ -29,14 +29,14 @@ typedef struct serialPort { struct serialPortVTable { void (*serialWrite)(serialPort_t *instance, uint8_t ch); - + uint8_t (*serialTotalBytesWaiting)(serialPort_t *instance); - + uint8_t (*serialRead)(serialPort_t *instance); - + // Specified baud rate may not be allowed by an implementation, use serialGetBaudRate to determine actual baud rate in use. void (*serialSetBaudRate)(serialPort_t *instance, uint32_t baudRate); - + bool (*isSerialTransmitBufferEmpty)(serialPort_t *instance); void (*setMode)(serialPort_t *instance, portMode_t mode); diff --git a/src/drv_softserial.c b/src/drv_softserial.c index 44eb7882ed..aa94f03c21 100644 --- a/src/drv_softserial.c +++ b/src/drv_softserial.c @@ -96,7 +96,7 @@ void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t refere void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) { - TIM_ICInitTypeDef TIM_ICInitStructure; + TIM_ICInitTypeDef TIM_ICInitStructure; TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_Channel = channel; @@ -253,7 +253,7 @@ void applyChangedBits(softSerial_t *softSerial) { if (softSerial->rxEdge == TRAILING) { uint8_t bitToSet; - for (bitToSet = softSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < softSerial->rxBitIndex ; bitToSet++) { + for (bitToSet = softSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < softSerial->rxBitIndex; bitToSet++) { softSerial->internalRxBuffer |= 1 << bitToSet; } } diff --git a/src/drv_system.c b/src/drv_system.c index f7f1f713f9..9fb2456968 100755 --- a/src/drv_system.c +++ b/src/drv_system.c @@ -10,7 +10,7 @@ void SetSysClock(bool overclock); void systemBeep(bool onoff); static void beepRev4(bool onoff); static void beepRev5(bool onoff); -void (* systemBeepPtr)(bool onoff) = NULL; +void (*systemBeepPtr)(bool onoff) = NULL; #endif static void cycleCounterInit(void) diff --git a/src/drv_timer.c b/src/drv_timer.c index ed6ae13462..71bbc863f6 100644 --- a/src/drv_timer.c +++ b/src/drv_timer.c @@ -147,7 +147,7 @@ void timerNVICConfigure(uint8_t irq) void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) { - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Period = period - 1; // AKA TIMx_ARR diff --git a/src/drv_uart.c b/src/drv_uart.c index 0bdc6253b2..2458e92f72 100755 --- a/src/drv_uart.c +++ b/src/drv_uart.c @@ -18,14 +18,14 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) s = &uartPort1; s->port.vTable = uartVTable; - + s->port.baudRate = baudRate; - + s->port.rxBuffer = rx1Buffer; s->port.txBuffer = tx1Buffer; s->port.rxBufferSize = UART1_RX_BUFFER_SIZE; s->port.txBufferSize = UART1_TX_BUFFER_SIZE; - + s->rxDMAChannel = DMA1_Channel5; s->txDMAChannel = DMA1_Channel4; @@ -63,14 +63,14 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode) s = &uartPort2; s->port.vTable = uartVTable; - + s->port.baudRate = baudRate; - + s->port.rxBufferSize = UART2_RX_BUFFER_SIZE; s->port.txBufferSize = UART2_TX_BUFFER_SIZE; s->port.rxBuffer = rx2Buffer; s->port.txBuffer = tx2Buffer; - + s->USARTx = USART2; RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); @@ -100,7 +100,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, { DMA_InitTypeDef DMA_InitStructure; USART_InitTypeDef USART_InitStructure; - + uartPort_t *s = NULL; if (USARTx == USART1) @@ -109,7 +109,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, s = serialUSART2(baudRate, mode); s->USARTx = USARTx; - + // common serial initialisation code should move to serialPort::init() s->port.rxBufferHead = s->port.rxBufferTail = 0; s->port.txBufferHead = s->port.txBufferTail = 0; @@ -185,7 +185,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, void uartSetBaudRate(serialPort_t *instance, uint32_t baudRate) { USART_InitTypeDef USART_InitStructure; - uartPort_t *s = (uartPort_t *)instance; + uartPort_t *s = (uartPort_t *)instance; USART_InitStructure.USART_BaudRate = baudRate; USART_InitStructure.USART_WordLength = USART_WordLength_8b; @@ -198,7 +198,7 @@ void uartSetBaudRate(serialPort_t *instance, uint32_t baudRate) if (s->port.mode & MODE_TX) USART_InitStructure.USART_Mode |= USART_Mode_Tx; USART_Init(s->USARTx, &USART_InitStructure); - + s->port.baudRate = baudRate; } diff --git a/src/drv_uart.h b/src/drv_uart.h index 3e1b3cd88e..58a1e0a2e4 100755 --- a/src/drv_uart.h +++ b/src/drv_uart.h @@ -12,7 +12,7 @@ // FIXME this is a uart_t really. Move the generic properties into a separate structure (serialPort_t) and update the code to use it typedef struct { serialPort_t port; - + // FIXME these are uart specific and do not belong in here DMA_Channel_TypeDef *rxDMAChannel; DMA_Channel_TypeDef *txDMAChannel; diff --git a/src/gps.c b/src/gps.c index 90690d0479..1afacf712e 100644 --- a/src/gps.c +++ b/src/gps.c @@ -57,10 +57,10 @@ typedef struct gpsData_t { int errors; // gps error counter - crc error/lost of data/sync etc. reset on each reinit. uint32_t lastMessage; // last time valid GPS data was received (millis) uint32_t lastLastMessage; // last-last valid GPS message. Used to calculate delta. - + uint32_t state_position; // incremental variable for loops uint32_t state_ts; // timestamp for last state_position increment - + } gpsData_t; gpsData_t gpsData; @@ -436,7 +436,7 @@ static void gpsNewData(uint16_t c) #endif // dTnav calculation // Time for calculating x,y speed and navigation pids - dTnav = (float) (millis() - nav_loopTimer) / 1000.0f; + dTnav = (float)(millis() - nav_loopTimer) / 1000.0f; nav_loopTimer = millis(); // prevent runup from bad GPS dTnav = min(dTnav, 1.0f); @@ -612,7 +612,7 @@ static bool check_missed_wp(void) static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing) { float dLat = *lat2 - *lat1; // difference of latitude in 1/10 000 000 degrees - float dLon = (float) (*lon2 - *lon1) * GPS_scaleLonDown; + float dLon = (float)(*lon2 - *lon1) * GPS_scaleLonDown; *dist = sqrtf(sq(dLat) + sq(dLon)) * 1.113195f; *bearing = 9000.0f + atan2f(-dLat, dLon) * 5729.57795f; // Convert the output radians to 100xdeg @@ -644,8 +644,8 @@ static void GPS_calc_velocity(void) if (init) { float tmp = 1.0f / dTnav; - actual_speed[GPS_X] = (float) (GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp; - actual_speed[GPS_Y] = (float) (GPS_coord[LAT] - last[LAT]) * tmp; + actual_speed[GPS_X] = (float)(GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp; + actual_speed[GPS_Y] = (float)(GPS_coord[LAT] - last[LAT]) * tmp; actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2; actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2; @@ -670,7 +670,7 @@ static void GPS_calc_velocity(void) // static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, int32_t *gps_lat, int32_t *gps_lng) { - error[LON] = (float) (*target_lng - *gps_lng) * GPS_scaleLonDown; // X Error + error[LON] = (float)(*target_lng - *gps_lng) * GPS_scaleLonDown; // X Error error[LAT] = *target_lat - *gps_lat; // Y Error } @@ -698,7 +698,7 @@ static void GPS_calc_poshold(void) d = 0; #endif - nav[axis] +=d; + nav[axis] += d; nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX); navPID[axis].integrator = poshold_ratePID[axis].integrator; } @@ -774,7 +774,7 @@ static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow) // limit the ramp up of the speed // waypoint_speed_gov is reset to 0 at each new WP command if (max_speed > waypoint_speed_gov) { - waypoint_speed_gov += (int) (100.0f * dTnav); // increase at .5/ms + waypoint_speed_gov += (int)(100.0f * dTnav); // increase at .5/ms max_speed = waypoint_speed_gov; } return max_speed; @@ -898,7 +898,7 @@ static uint32_t grab_fields(char *src, uint8_t mult) tmp *= 10; if (src[i] >= '0' && src[i] <= '9') tmp += src[i] - '0'; - if (i >= 15) + if (i >= 15) return 0; // out of bounds } return tmp; @@ -1056,8 +1056,7 @@ typedef struct { uint32_t heading_accuracy; } ubx_nav_velned; -typedef struct -{ +typedef struct { uint8_t chn; // Channel number, 255 for SVx not assigned to channel uint8_t svid; // Satellite ID uint8_t flags; // Bitmask @@ -1068,8 +1067,7 @@ typedef struct int32_t prRes; // Pseudo range residual in centimetres } ubx_nav_svinfo_channel; -typedef struct -{ +typedef struct { uint32_t time; // GPS Millisecond time of week uint8_t numCh; // Number of channels uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6 @@ -1180,7 +1178,7 @@ static bool gpsNewFrameUBLOX(uint8_t data) case 5: _step++; _ck_b += (_ck_a += data); // checksum byte - _payload_length += (uint16_t) (data << 8); + _payload_length += (uint16_t)(data << 8); if (_payload_length > 512) { _payload_length = 0; _step = 0; diff --git a/src/imu.c b/src/imu.c index 7d45b6ac8d..11cadfb949 100644 --- a/src/imu.c +++ b/src/imu.c @@ -391,7 +391,7 @@ int getEstimatedAltitude(void) // set vario vario = applyDeadband(vel_tmp, 5); - + if (abs(angle[ROLL]) < 800 && abs(angle[PITCH]) < 800) { // only calculate pid if the copters thrust is facing downwards(<80deg) // Altitude P-Controller error = constrain(AltHold - EstAlt, -500, 500); @@ -410,11 +410,11 @@ int getEstimatedAltitude(void) // D BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150); - + } else { BaroPID = 0; } - + accZ_old = accZ_tmp; return 1; diff --git a/src/mixer.c b/src/mixer.c index 8597e460fc..cfbff6e07f 100755 --- a/src/mixer.c +++ b/src/mixer.c @@ -430,7 +430,7 @@ void mixTable(void) break; case MULTITYPE_DUALCOPTER: - for (i = 4; i < 6; i++ ) { + for (i = 4; i < 6; i++) { servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction servo[i] += servoMiddle(i); } diff --git a/src/mw.c b/src/mw.c index 43a0881ae1..ba4b1203b2 100755 --- a/src/mw.c +++ b/src/mw.c @@ -363,9 +363,9 @@ static void pidRewrite(void) } if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) AngleRateTmp = (((int32_t)(cfg.yawRate + 27) * rcCommand[2]) >> 5); - } else { + } else { if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID - AngleRateTmp = ((int32_t) (cfg.rollPitchRate + 27) * rcCommand[axis]) >> 4; + AngleRateTmp = ((int32_t)(cfg.rollPitchRate + 27) * rcCommand[axis]) >> 4; if (f.HORIZON_MODE) { // mix up angle error to desired AngleRateTmp to add a little auto-level feel AngleRateTmp += (errorAngle * cfg.I8[PIDLEVEL]) >> 8; @@ -625,7 +625,7 @@ void loop(void) if (rcOptions[BOXCALIB]) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone) InflightcalibratingA = 50; - AccInflightCalibrationActive = true; + AccInflightCalibrationActive = true; } else if (AccInflightCalibrationMeasurementDone && !f.ARMED) { AccInflightCalibrationMeasurementDone = false; AccInflightCalibrationSavetoEEProm = true; diff --git a/src/sbus.c b/src/sbus.c index 7fda225dc3..9e2332bdde 100644 --- a/src/sbus.c +++ b/src/sbus.c @@ -28,8 +28,7 @@ void sbusInit(rcReadRawDataPtr *callback) core.numRCChannels = SBUS_MAX_CHANNEL; } -struct sbus_dat -{ +struct sbus_dat { unsigned int chan0 : 11; unsigned int chan1 : 11; unsigned int chan2 : 11; @@ -44,8 +43,7 @@ struct sbus_dat unsigned int chan11 : 11; } __attribute__ ((__packed__)); -typedef union -{ +typedef union { uint8_t in[SBUS_FRAME_SIZE]; struct sbus_dat msg; } sbus_msg; @@ -57,7 +55,7 @@ static void sbusDataReceive(uint16_t c) { uint32_t sbusTime; static uint32_t sbusTimeLast; - static uint8_t sbusFramePosition; + static uint8_t sbusFramePosition; sbusTime = micros(); if ((sbusTime - sbusTimeLast) > 2500) // sbus2 fast timing diff --git a/src/serial.c b/src/serial.c index cb6f5b2d1b..e6b212f88e 100755 --- a/src/serial.c +++ b/src/serial.c @@ -158,14 +158,14 @@ uint8_t read8(void) uint16_t read16(void) { uint16_t t = read8(); - t += (uint16_t) read8() << 8; + t += (uint16_t)read8() << 8; return t; } uint32_t read32(void) { uint32_t t = read16(); - t += (uint32_t) read16() << 16; + t += (uint32_t)read16() << 16; return t; } diff --git a/src/sumd.c b/src/sumd.c index 77bcfe8ea5..d230029134 100644 --- a/src/sumd.c +++ b/src/sumd.c @@ -31,17 +31,17 @@ static void sumdDataReceive(uint16_t c) static uint8_t sumdIndex; sumdTime = micros(); - if ((sumdTime - sumdTimeLast) > 4000) + if ((sumdTime - sumdTimeLast) > 4000) sumdIndex = 0; sumdTimeLast = sumdTime; if (sumdIndex == 0) { - if (c != SUMD_SYNCBYTE) + if (c != SUMD_SYNCBYTE) return; else sumdFrameDone = false; // lazy main loop didnt fetch the stuff } - if (sumdIndex == 2) + if (sumdIndex == 2) sumdSize = (uint8_t)c; if (sumdIndex < SUMD_BUFFSIZE) sumd[sumdIndex] = (uint8_t)c; diff --git a/src/telemetry_hott.c b/src/telemetry_hott.c index 8b58a9acad..37a6c120c0 100644 --- a/src/telemetry_hott.c +++ b/src/telemetry_hott.c @@ -88,13 +88,13 @@ void hottV4FormatAndSendGPSResponse(void) void hottV4GPSUpdate(void) { // Number of Satelites - HoTTV4GPSModule.GPSNumSat=GPS_numSat; + HoTTV4GPSModule.GPSNumSat = GPS_numSat; if (f.GPS_FIX > 0) { // GPS fix HoTTV4GPSModule.GPS_fix = 0x66; // Displays a 'f' for fix // latitude - HoTTV4GPSModule.LatitudeNS=(GPS_coord[LAT]<0); + HoTTV4GPSModule.LatitudeNS = (GPS_coord[LAT] < 0); uint8_t deg = GPS_coord[LAT] / 100000; uint32_t sec = (GPS_coord[LAT] - (deg * 100000)) * 6; uint8_t min = sec / 10000; @@ -106,7 +106,7 @@ void hottV4GPSUpdate(void) HoTTV4GPSModule.LatitudeSecHigh = sec >> 8; // longitude - HoTTV4GPSModule.longitudeEW=(GPS_coord[LON]<0); + HoTTV4GPSModule.longitudeEW = (GPS_coord[LON] < 0); deg = GPS_coord[LON] / 100000; sec = (GPS_coord[LON] - (deg * 100000)) * 6; min = sec / 10000; @@ -265,7 +265,8 @@ void handleHoTTTelemetry(void) switch (c) { case 0x8A: - if (sensors(SENSOR_GPS)) hottV4FormatAndSendGPSResponse(); + if (sensors(SENSOR_GPS)) + hottV4FormatAndSendGPSResponse(); break; case 0x8E: hottV4FormatAndSendEAMResponse();