diff --git a/src/imu.c b/src/imu.c index 91637cffc7..d4eb62a2f9 100644 --- a/src/imu.c +++ b/src/imu.c @@ -191,7 +191,7 @@ void acc_calc(uint32_t deltaT) accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis } else accel_ned.V.Z -= acc_1G; - + accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter // apply Deadband to reduce integration drift and vibration influence @@ -339,7 +339,7 @@ int getEstimatedAltitude(void) baroGroundPressure -= baroGroundPressure / 8; baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1); baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f; - + vel = 0; accAlt = 0; calibratingB--; @@ -358,8 +358,8 @@ int getEstimatedAltitude(void) vel_acc = accZ_tmp * accVelScale * (float)accTimeSum; // Integrator - Altitude in cm - accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) - accAlt = accAlt * cfg.baro_cf_alt + (float) BaroAlt *(1.0f - cfg.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc) + accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) + accAlt = accAlt * cfg.baro_cf_alt + (float)BaroAlt * (1.0f - cfg.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc) EstAlt = accAlt; vel += vel_acc; diff --git a/src/main.c b/src/main.c index bd0877ecab..58f60eb092 100755 --- a/src/main.c +++ b/src/main.c @@ -88,7 +88,7 @@ int main(void) break; default: pwm_params.adcChannel = 0; - break; + break; } pwmInit(&pwm_params); diff --git a/src/mw.c b/src/mw.c index b75cca42ed..e408e46cf0 100755 --- a/src/mw.c +++ b/src/mw.c @@ -51,11 +51,11 @@ uint16_t GPS_ground_course = 0; // degrees * 10 int16_t nav[2]; int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother int8_t nav_mode = NAV_MODE_NONE; // Navigation mode -uint8_t GPS_numCh; // Number of channels -uint8_t GPS_svinfo_chn[16]; // Channel number -uint8_t GPS_svinfo_svid[16]; // Satellite ID -uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity -uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength) +uint8_t GPS_numCh; // Number of channels +uint8_t GPS_svinfo_chn[16]; // Channel number +uint8_t GPS_svinfo_svid[16]; // Satellite ID +uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity +uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength) // Automatic ACC Offset Calibration uint16_t InflightcalibratingA = 0; @@ -103,7 +103,7 @@ void annexCode(void) prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { - prop2 = 100 - (uint16_t) cfg.dynThrPID * (rcData[THROTTLE] - cfg.tpaBreakPoint) / (2000 - cfg.tpaBreakPoint); + prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - cfg.tpaBreakPoint) / (2000 - cfg.tpaBreakPoint); } else { prop2 = 100 - cfg.dynThrPID; } @@ -143,7 +143,7 @@ void annexCode(void) } tmp = constrain(rcData[THROTTLE], mcfg.mincheck, 2000); - tmp = (uint32_t) (tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck); // [MINCHECK;2000] -> [0;1000] + tmp = (uint32_t)(tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck); // [MINCHECK;2000] -> [0;1000] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] @@ -363,7 +363,7 @@ static void pidRewrite(void) // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { // -----Get the desired angle rate depending on flight mode - if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2 ) { // MODE relying on ACC + if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC // calculate error and limit the angle to max configured inclination errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)mcfg.max_angle_inclination), +mcfg.max_angle_inclination) - angle[axis] + cfg.angleTrim[axis]; // 16 bits is ok here } diff --git a/src/sbus.c b/src/sbus.c index ae374af475..7fda225dc3 100644 --- a/src/sbus.c +++ b/src/sbus.c @@ -20,7 +20,7 @@ static uint32_t sbusChannelData[SBUS_MAX_CHANNEL]; void sbusInit(rcReadRawDataPtr *callback) { int b; - for (b = 0; b < SBUS_MAX_CHANNEL; b ++) + for (b = 0; b < SBUS_MAX_CHANNEL; b++) sbusChannelData[b] = 2 * (mcfg.midrc - SBUS_OFFSET); core.rcvrport = uartOpen(USART2, sbusDataReceive, 100000, (portMode_t)(MODE_RX | MODE_SBUS)); if (callback) @@ -49,9 +49,9 @@ typedef union uint8_t in[SBUS_FRAME_SIZE]; struct sbus_dat msg; } sbus_msg; - + static sbus_msg sbus; - + // Receive ISR callback static void sbusDataReceive(uint16_t c) { @@ -63,12 +63,12 @@ static void sbusDataReceive(uint16_t c) if ((sbusTime - sbusTimeLast) > 2500) // sbus2 fast timing sbusFramePosition = 0; sbusTimeLast = sbusTime; - + if (sbusFramePosition == 0 && c != SBUS_SYNCBYTE) return; sbusFrameDone = false; // lazy main loop didnt fetch the stuff - if (sbusFramePosition != 0) + if (sbusFramePosition != 0) sbus.in[sbusFramePosition - 1] = (uint8_t)c; if (sbusFramePosition == SBUS_FRAME_SIZE - 1) { diff --git a/src/sensors.c b/src/sensors.c index ed3bd94c35..cdc1a80019 100755 --- a/src/sensors.c +++ b/src/sensors.c @@ -179,8 +179,8 @@ static void ACC_Common(void) // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration if (calibratingA == 1) { mcfg.accZero[ROLL] = (a[ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; - mcfg.accZero[PITCH] = (a[PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; - mcfg.accZero[YAW] = (a[YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G; + mcfg.accZero[PITCH] = (a[PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; + mcfg.accZero[YAW] = (a[YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G; cfg.angleTrim[ROLL] = 0; cfg.angleTrim[PITCH] = 0; writeEEPROM(1, true); // write accZero in EEPROM @@ -264,7 +264,6 @@ void Baro_Common(void) baroHistIdx = indexplus1; } - int Baro_update(void) { static uint32_t baroDeadline = 0; diff --git a/src/sumd.c b/src/sumd.c index 97d27a4ad4..77bcfe8ea5 100644 --- a/src/sumd.c +++ b/src/sumd.c @@ -16,7 +16,7 @@ static uint32_t sumdChannelData[SUMD_MAX_CHANNEL]; void sumdInit(rcReadRawDataPtr *callback) { core.rcvrport = uartOpen(USART2, sumdDataReceive, 115200, MODE_RX); - if (callback) + if (callback) *callback = sumdReadRawRC; } @@ -60,7 +60,7 @@ bool sumdFrameComplete(void) sumdFrameDone = false; if (sumd[1] == 0x01) { failsafeCnt = 0; - if (sumdSize > SUMD_MAX_CHANNEL) + if (sumdSize > SUMD_MAX_CHANNEL) sumdSize = SUMD_MAX_CHANNEL; for (b = 0; b < sumdSize; b++) sumdChannelData[b] = ((sumd[2 * b + 3] << 8) | sumd[2 * b + 4]); diff --git a/src/utils.c b/src/utils.c index db0b2697d9..f174069dc3 100644 --- a/src/utils.c +++ b/src/utils.c @@ -30,7 +30,7 @@ void initBoardAlignment(void) roll = mcfg.board_align_roll * M_PI / 180.0f; pitch = mcfg.board_align_pitch * M_PI / 180.0f; yaw = mcfg.board_align_yaw * M_PI / 180.0f; - + cosx = cosf(roll); sinx = sinf(roll); cosy = cosf(pitch); @@ -48,11 +48,11 @@ void initBoardAlignment(void) boardRotation[0][0] = coszcosy; boardRotation[0][1] = -cosy * sinz; boardRotation[0][2] = siny; - + boardRotation[1][0] = sinzcosx + (coszsinx * siny); boardRotation[1][1] = coszcosx - (sinzsinx * siny); boardRotation[1][2] = -sinx * cosy; - + boardRotation[2][0] = (sinzsinx) - (coszcosx * siny); boardRotation[2][1] = (coszsinx) + (sinzcosx * siny); boardRotation[2][2] = cosy * cosx;