diff --git a/src/buzzer.c b/src/buzzer.c index 243f2047e..e2cd71699 100644 --- a/src/buzzer.c +++ b/src/buzzer.c @@ -113,7 +113,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; diff --git a/src/common/typeconversion.c b/src/common/typeconversion.c index d6d45934f..db8673256 100644 --- a/src/common/typeconversion.c +++ b/src/common/typeconversion.c @@ -93,18 +93,18 @@ char a2i(char ch, char **src, int base, int *nump) #ifndef HAVE_ITOA_FUNCTION /* -** The following two functions together make up an itoa() -** implementation. Function i2a() is a 'private' function -** called by the public itoa() function. -** -** itoa() takes three arguments: -** 1) the integer to be converted, -** 2) a pointer to a character conversion buffer, -** 3) the radix for the conversion -** which can range between 2 and 36 inclusive -** range errors on the radix default it to base10 -** Code from http://groups.google.com/group/comp.lang.c/msg/66552ef8b04fe1ab?pli=1 -*/ + ** The following two functions together make up an itoa() + ** implementation. Function i2a() is a 'private' function + ** called by the public itoa() function. + ** + ** itoa() takes three arguments: + ** 1) the integer to be converted, + ** 2) a pointer to a character conversion buffer, + ** 3) the radix for the conversion + ** which can range between 2 and 36 inclusive + ** range errors on the radix default it to base10 + ** Code from http://groups.google.com/group/comp.lang.c/msg/66552ef8b04fe1ab?pli=1 + */ static char *_i2a(unsigned i, char *a, unsigned r) { @@ -120,7 +120,7 @@ char *itoa(int i, char *a, int r) r = 10; if (i < 0) { *a = '-'; - *_i2a(-(unsigned)i, a + 1, r) = 0; + *_i2a(-(unsigned) i, a + 1, r) = 0; } else *_i2a(i, a, r) = 0; return a; @@ -141,7 +141,7 @@ 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 @@ -176,7 +176,6 @@ char *ftoa(float x, char *floatString) return floatString; } - // Simple and fast atof (ascii to float) function. // // - Executes about 5x faster than standard MSCRT library atof(). @@ -194,7 +193,7 @@ float fastA2F(const char *p) float sign, value, scale; // Skip leading white space, if any. - while (white_space(*p) ) { + while (white_space(*p)) { p += 1; } @@ -254,8 +253,14 @@ float fastA2F(const char *p) // Calculate scaling factor. // while (expon >= 50) { scale *= 1E50f; expon -= 50; } - while (expon >= 8) { scale *= 1E8f; expon -= 8; } - while (expon > 0) { scale *= 10.0f; expon -= 1; } + while (expon >= 8) { + scale *= 1E8f; + expon -= 8; + } + while (expon > 0) { + scale *= 10.0f; + expon -= 1; + } } // Return signed and scaled floating point result. diff --git a/src/config.c b/src/config.c index cc0cb9bb8..1210afa58 100755 --- a/src/config.c +++ b/src/config.c @@ -37,22 +37,21 @@ #include "config_master.h" void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h -void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfig, gimbalConfig_t *gimbalConfigToUse); - +void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, + escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, + airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfig, gimbalConfig_t *gimbalConfigToUse); #ifndef FLASH_PAGE_COUNT #define FLASH_PAGE_COUNT 128 #endif #define FLASH_PAGE_SIZE ((uint16_t)0x400) -#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 2)) // use the last 2 KB for storage - -master_t masterConfig; // master config struct with data independent from profiles +#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 2)) // use the last 2 KB for storage +master_t masterConfig; // master config struct with data independent from profiles profile_t currentProfile; // profile config struct static const uint8_t EEPROM_CONF_VERSION = 66; - static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims) { accelerometerTrims->trims.pitch = 0; @@ -168,10 +167,10 @@ static void resetConf(void) featureSet(FEATURE_VBAT); // global settings - masterConfig.current_profile_index = 0; // default profile - masterConfig.gyro_cmpf_factor = 600; // default MWC - masterConfig.gyro_cmpfm_factor = 250; // default MWC - masterConfig.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead + masterConfig.current_profile_index = 0; // default profile + masterConfig.gyro_cmpf_factor = 600; // default MWC + masterConfig.gyro_cmpfm_factor = 250; // default MWC + masterConfig.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead resetAccelerometerTrims(&masterConfig.accZero); @@ -195,7 +194,7 @@ static void resetConf(void) masterConfig.rxConfig.midrc = 1500; masterConfig.rxConfig.mincheck = 1100; masterConfig.rxConfig.maxcheck = 1900; - masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right + masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right masterConfig.airplaneConfig.flaps_speed = 0; masterConfig.fixedwing_althold_dir = 1; @@ -250,10 +249,10 @@ static void resetConf(void) currentProfile.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv // Failsafe Variables - currentProfile.failsafeConfig.failsafe_delay = 10; // 1sec - currentProfile.failsafeConfig.failsafe_off_delay = 200; // 20sec - currentProfile.failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people. - currentProfile.failsafeConfig.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe + currentProfile.failsafeConfig.failsafe_delay = 10; // 1sec + currentProfile.failsafeConfig.failsafe_off_delay = 200; // 20sec + currentProfile.failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people. + currentProfile.failsafeConfig.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe // servos for (i = 0; i < 8; i++) { @@ -293,7 +292,7 @@ static uint8_t calculateChecksum(const uint8_t *data, uint32_t length) static bool isEEPROMContentValid(void) { - const master_t *temp = (const master_t *)FLASH_WRITE_ADDR; + const master_t *temp = (const master_t *) FLASH_WRITE_ADDR; uint8_t checksum = 0; // check version number @@ -305,7 +304,7 @@ static bool isEEPROMContentValid(void) return false; // verify integrity of temporary copy - checksum = calculateChecksum((const uint8_t *)temp, sizeof(master_t)); + checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t)); if (checksum != 0) return false; @@ -326,21 +325,22 @@ void activateConfig(void) useFailsafeConfig(¤tProfile.failsafeConfig); setAccelerationTrims(&masterConfig.accZero); mixerUseConfigs( - currentProfile.servoConf, - &masterConfig.flight3DConfig, - &masterConfig.escAndServoConfig, - ¤tProfile.mixerConfig, - &masterConfig.airplaneConfig, - &masterConfig.rxConfig, - ¤tProfile.gimbalConfig - ); + currentProfile.servoConf, + &masterConfig.flight3DConfig, + &masterConfig.escAndServoConfig, + ¤tProfile.mixerConfig, + &masterConfig.airplaneConfig, + &masterConfig.rxConfig, + ¤tProfile.gimbalConfig + ); #ifdef BARO useBarometerConfig(¤tProfile.barometerConfig); #endif } -void validateAndFixConfig(void) { +void validateAndFixConfig(void) +{ if (!(feature(FEATURE_PARALLEL_PWM) || feature(FEATURE_PPM) || feature(FEATURE_SERIALRX))) { featureSet(FEATURE_PARALLEL_PWM); // Consider changing the default to PPM } @@ -390,7 +390,7 @@ void readEEPROM(void) failureMode(10); // Read flash - memcpy(&masterConfig, (char *)FLASH_WRITE_ADDR, sizeof(master_t)); + memcpy(&masterConfig, (char *) FLASH_WRITE_ADDR, sizeof(master_t)); // Copy current profile if (masterConfig.current_profile_index > 2) // sanity check masterConfig.current_profile_index = 0; @@ -425,7 +425,7 @@ void writeEEPROM(void) masterConfig.magic_be = 0xBE; masterConfig.magic_ef = 0xEF; masterConfig.chk = 0; // erase checksum before recalculating - masterConfig.chk = calculateChecksum((const uint8_t *)&masterConfig, sizeof(master_t)); + masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t)); // write it FLASH_Unlock(); @@ -438,7 +438,8 @@ void writeEEPROM(void) #endif status = FLASH_ErasePage(FLASH_WRITE_ADDR); for (wordOffset = 0; wordOffset < sizeof(master_t) && status == FLASH_COMPLETE; wordOffset += 4) { - status = FLASH_ProgramWord(FLASH_WRITE_ADDR + wordOffset, *(uint32_t *) ((char *)&masterConfig + wordOffset)); + status = FLASH_ProgramWord(FLASH_WRITE_ADDR + wordOffset, + *(uint32_t *) ((char *) &masterConfig + wordOffset)); } if (status == FLASH_COMPLETE) { break; diff --git a/src/drivers/accgyro_adxl345.c b/src/drivers/accgyro_adxl345.c index deb426f12..51b1e8a99 100755 --- a/src/drivers/accgyro_adxl345.c +++ b/src/drivers/accgyro_adxl345.c @@ -66,7 +66,7 @@ bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc) static void adxl345Init(void) { - 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/drivers/accgyro_common.h b/src/drivers/accgyro_common.h index 95daf2a76..8366e3c4d 100644 --- a/src/drivers/accgyro_common.h +++ b/src/drivers/accgyro_common.h @@ -2,19 +2,17 @@ extern uint16_t acc_1G; -typedef void (* sensorInitFuncPtr)(void); // sensor init prototype -typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype +typedef void (*sensorInitFuncPtr)(void); // sensor init prototype +typedef void (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype -typedef struct gyro_s -{ +typedef struct gyro_s { sensorInitFuncPtr init; // initialize function sensorReadFuncPtr read; // read 3 axis data function sensorReadFuncPtr temperature; // read temperature if available float scale; // scalefactor } gyro_t; -typedef struct acc_s -{ +typedef struct acc_s { sensorInitFuncPtr init; // initialize function sensorReadFuncPtr read; // read 3 axis data function char revisionCode; // a revision code for the sensor, if known diff --git a/src/drivers/barometer_bmp085.c b/src/drivers/barometer_bmp085.c index daaa958c7..b1cfb9dbe 100755 --- a/src/drivers/barometer_bmp085.c +++ b/src/drivers/barometer_bmp085.c @@ -39,7 +39,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; @@ -198,9 +198,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) { @@ -258,7 +258,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/drivers/barometer_common.h b/src/drivers/barometer_common.h index 924f4194b..702d881b3 100644 --- a/src/drivers/barometer_common.h +++ b/src/drivers/barometer_common.h @@ -1,10 +1,9 @@ #pragma once -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 (*baroOpFuncPtr)(void); // baro start operation +typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature) -typedef struct baro_t -{ +typedef struct baro_t { uint16_t ut_delay; uint16_t up_delay; baroOpFuncPtr start_ut; diff --git a/src/drivers/barometer_ms5611.c b/src/drivers/barometer_ms5611.c index 930092f09..9c6b49ae7 100644 --- a/src/drivers/barometer_ms5611.c +++ b/src/drivers/barometer_ms5611.c @@ -165,11 +165,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; @@ -179,11 +179,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/drivers/gpio_stm32f10x.c b/src/drivers/gpio_stm32f10x.c index 903a67fad..f536e74d6 100644 --- a/src/drivers/gpio_stm32f10x.c +++ b/src/drivers/gpio_stm32f10x.c @@ -77,7 +77,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/drivers/pwm_fy90q.c b/src/drivers/pwm_fy90q.c index 40322307d..b7134c9d1 100644 --- a/src/drivers/pwm_fy90q.c +++ b/src/drivers/pwm_fy90q.c @@ -56,7 +56,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; @@ -158,11 +158,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; @@ -246,7 +246,7 @@ static void pwmInitializeInput(bool usePPM) void 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/drivers/pwm_output.c b/src/drivers/pwm_output.c index 0099d9e22..219645e3e 100644 --- a/src/drivers/pwm_output.c +++ b/src/drivers/pwm_output.c @@ -14,7 +14,7 @@ #include "pwm_output.h" -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 typedef struct { #ifdef STM32F303xC diff --git a/src/drivers/serial_common.h b/src/drivers/serial_common.h index 0763b2443..7f4e1dde0 100644 --- a/src/drivers/serial_common.h +++ b/src/drivers/serial_common.h @@ -7,17 +7,18 @@ typedef enum { } serialInversion_e; 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 void (* serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app +typedef void (*serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app typedef struct serialPort { - + const struct serialPortVTable *vTable; + uint8_t identifier; portMode_t mode; serialInversion_e inversion; @@ -38,14 +39,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/drivers/serial_softserial.c b/src/drivers/serial_softserial.c index dca435982..1197288b4 100644 --- a/src/drivers/serial_softserial.c +++ b/src/drivers/serial_softserial.c @@ -91,7 +91,7 @@ static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t static 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; @@ -231,7 +231,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/drivers/serial_uart_common.h b/src/drivers/serial_uart_common.h index 62221dd7f..33bcaf67e 100755 --- a/src/drivers/serial_uart_common.h +++ b/src/drivers/serial_uart_common.h @@ -8,7 +8,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/drivers/sonar_hcsr04.c b/src/drivers/sonar_hcsr04.c index feb844ea6..72b8c4940 100644 --- a/src/drivers/sonar_hcsr04.c +++ b/src/drivers/sonar_hcsr04.c @@ -69,7 +69,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/drivers/sound_beeper.c b/src/drivers/sound_beeper.c index d586afaa5..d8cac315d 100644 --- a/src/drivers/sound_beeper.c +++ b/src/drivers/sound_beeper.c @@ -13,7 +13,7 @@ #ifdef BUZZER -void (* systemBeepPtr)(bool onoff) = NULL; +void (*systemBeepPtr)(bool onoff) = NULL; static void beepNormal(bool onoff) { diff --git a/src/flight_common.c b/src/flight_common.c index 748198492..cbd4ee2bc 100644 --- a/src/flight_common.c +++ b/src/flight_common.c @@ -17,17 +17,17 @@ int16_t heading, magHold; int16_t axisPID[3]; uint8_t dynP8[3], dynI8[3], dynD8[3]; - static int32_t errorGyroI[3] = { 0, 0, 0 }; static int32_t errorAngleI[2] = { 0, 0 }; -static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); +static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); -typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); // pid controller function prototype +typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); // pid controller function prototype pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii - void mwDisarm(void) { if (f.ARMED) @@ -53,7 +53,8 @@ void resetErrorGyro(void) errorGyroI[YAW] = 0; } -static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim) +static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim) { int axis, prop; int32_t error, errorAngle; @@ -68,7 +69,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa for (axis = 0; axis < 3; axis++) { if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC // 50 degrees max inclination - errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; + errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5); @@ -76,7 +78,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12; } if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == FD_YAW) { // MODE relying on GYRO or YAW axis - error = (int32_t)rcCommand[axis] * 10 * 8 / pidProfile->P8[axis]; + error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis]; error -= gyroData[axis]; PTermGYRO = rcCommand[axis]; @@ -99,7 +101,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } } - PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation + PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation delta = gyroData[axis] - lastGyro[axis]; lastGyro[axis] = gyroData[axis]; deltaSum = delta1[axis] + delta2[axis] + delta; @@ -112,7 +114,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa #define GYRO_I_MAX 256 -static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim) +static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, + rollAndPitchTrims_t *angleTrim) { int32_t errorAngle = 0; int axis; @@ -127,13 +130,14 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // -----Get the desired angle rate depending on flight mode if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { // MODE relying on ACC // calculate error and limit the angle to max configured inclination - errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; // 16 bits is ok here + errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; // 16 bits is ok here } if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) AngleRateTmp = (((int32_t)(controlRateConfig->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) (controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4; + AngleRateTmp = ((int32_t)(controlRateConfig->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 * pidProfile->I8[PIDLEVEL]) >> 8; @@ -160,16 +164,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings - errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13); + errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); ITerm = errorGyroI[axis] >> 13; //-----calculate D-term - delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 lastError[axis] = RateError; // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // would be scaled by different dt each time. Division by dT fixes that. - delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6; + delta = (delta * ((uint16_t) 0xFFFF / (cycleTime >> 4))) >> 6; // add moving average here to reduce noise deltaSum = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; diff --git a/src/flight_imu.c b/src/flight_imu.c index dda31c41b..3f31499f8 100644 --- a/src/flight_imu.c +++ b/src/flight_imu.c @@ -458,6 +458,7 @@ int getEstimatedAltitude(void) vario = applyDeadband(vel_tmp, 5); BaroPID = calculateBaroPid(vel_tmp, accZ_tmp, accZ_old); + accZ_old = accZ_tmp; return 1; diff --git a/src/flight_mixer.c b/src/flight_mixer.c index 2b4428a01..878c0b75a 100755 --- a/src/flight_mixer.c +++ b/src/flight_mixer.c @@ -478,7 +478,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] += determineServoMiddleOrForwardFromChannel(i); } diff --git a/src/gps_common.c b/src/gps_common.c index 643ad2809..c621c134a 100644 --- a/src/gps_common.c +++ b/src/gps_common.c @@ -118,10 +118,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; static gpsData_t gpsData; @@ -495,7 +495,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); @@ -684,7 +684,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 @@ -716,8 +716,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; @@ -742,7 +742,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 } @@ -770,7 +770,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; } @@ -846,7 +846,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; @@ -970,7 +970,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; @@ -1128,8 +1128,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 @@ -1140,8 +1139,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 @@ -1252,7 +1250,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/mw.c b/src/mw.c index 2022228f0..3a540117d 100755 --- a/src/mw.c +++ b/src/mw.c @@ -68,7 +68,7 @@ uint16_t rssi; // range: [0;1023] extern uint8_t dynP8[3], dynI8[3], dynD8[3]; extern failsafe_t *failsafe; -typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims); +typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims); extern pidControllerFuncPtr pid_controller; @@ -224,7 +224,7 @@ static void mwVario(void) { } - + void loop(void) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors @@ -395,7 +395,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/rx_common.h b/src/rx_common.h index c3b1b5ec6..8658872e3 100644 --- a/src/rx_common.h +++ b/src/rx_common.h @@ -38,7 +38,7 @@ typedef struct rxRuntimeConfig_s { extern rxRuntimeConfig_t rxRuntimeConfig; -typedef uint16_t (* rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data +typedef uint16_t (*rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/rx_sbus.c b/src/rx_sbus.c index 1d1905a08..b37f630b8 100644 --- a/src/rx_sbus.c +++ b/src/rx_sbus.c @@ -43,8 +43,7 @@ bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa return sBusPort != NULL; } -struct sbus_dat -{ +struct sbus_dat { unsigned int chan0 : 11; unsigned int chan1 : 11; unsigned int chan2 : 11; @@ -59,8 +58,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; @@ -72,7 +70,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/rx_sumd.c b/src/rx_sumd.c index 21b5697bd..9bc77409f 100644 --- a/src/rx_sumd.c +++ b/src/rx_sumd.c @@ -49,17 +49,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/serial_cli.c b/src/serial_cli.c index ffcfe6b29..7091d889e 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -303,7 +303,7 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value); static void cliPrintVar(const clivalue_t *var, uint32_t full); static void cliPrint(const char *str); static void cliWrite(uint8_t ch); - + static void cliPrompt(void) { cliPrint("\r\n# "); diff --git a/src/serial_msp.c b/src/serial_msp.c index 56ec495a0..b92252177 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -202,14 +202,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/telemetry_hott.c b/src/telemetry_hott.c index cf84bef6c..b8258695c 100644 --- a/src/telemetry_hott.c +++ b/src/telemetry_hott.c @@ -112,13 +112,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; @@ -130,7 +130,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; @@ -312,7 +312,8 @@ void handleHoTTTelemetry(void) switch (c) { case 0x8A: - if (sensors(SENSOR_GPS)) hottV4FormatAndSendGPSResponse(); + if (sensors(SENSOR_GPS)) + hottV4FormatAndSendGPSResponse(); break; case 0x8E: hottV4FormatAndSendEAMResponse();