diff --git a/src/flight_imu.c b/src/flight_imu.c index bba1905eba..6d03f9bb2e 100644 --- a/src/flight_imu.c +++ b/src/flight_imu.c @@ -269,10 +269,12 @@ static void getEstimatedAttitude(void) accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G); rotateV(&EstG.V, deltaGyroAngle); - if (sensors(SENSOR_MAG)) + if (sensors(SENSOR_MAG)) { rotateV(&EstM.V, deltaGyroAngle); - else + } else { rotateV(&EstN.V, deltaGyroAngle); + normalizeV(&EstN.V, &EstN.V); + } // Apply complimentary filter (Gyro drift correction) // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation. @@ -311,11 +313,11 @@ static void getEstimatedAttitude(void) if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg throttleAngleCorrection = 0; - } else { + } else { int angle = lrintf(acosf(cosZ) * throttleAngleScale); if (angle > 900) angle = 900; - throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / 900.0f * M_PI / 2.0f)) ; + throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))) ; } } @@ -352,7 +354,7 @@ int getEstimatedAltitude(void) if (calibratingB > 0) { baroGroundPressure -= baroGroundPressure / 8; baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1); - baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f; + baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f; vel = 0; accAlt = 0; diff --git a/src/mw.c b/src/mw.c index 1cbc4358e8..0db01d7779 100755 --- a/src/mw.c +++ b/src/mw.c @@ -60,11 +60,11 @@ uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength) // Automatic ACC Offset Calibration +bool AccInflightCalibrationArmed = false; +bool AccInflightCalibrationMeasurementDone = false; +bool AccInflightCalibrationSavetoEEProm = false; +bool AccInflightCalibrationActive = false; uint16_t InflightcalibratingA = 0; -int16_t AccInflightCalibrationArmed; -uint16_t AccInflightCalibrationMeasurementDone = 0; -uint16_t AccInflightCalibrationSavetoEEProm = 0; -uint16_t AccInflightCalibrationActive = 0; void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat) { @@ -120,8 +120,8 @@ void annexCode(void) tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; - prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500; - prop1 = (uint16_t) prop1 *prop2 / 100; + prop1 = 100 - (uint16_t)cfg.rollPitchRate * tmp / 500; + prop1 = (uint16_t)prop1 * prop2 / 100; } else { // YAW if (cfg.yawdeadband) { if (tmp > cfg.yawdeadband) { @@ -558,8 +558,8 @@ void loop(void) // Inflight ACC Calibration } else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) { if (AccInflightCalibrationMeasurementDone) { // trigger saving into eeprom after landing - AccInflightCalibrationMeasurementDone = 0; - AccInflightCalibrationSavetoEEProm = 1; + AccInflightCalibrationMeasurementDone = false; + AccInflightCalibrationSavetoEEProm = true; } else { AccInflightCalibrationArmed = !AccInflightCalibrationArmed; if (AccInflightCalibrationArmed) { @@ -621,14 +621,15 @@ void loop(void) if (feature(FEATURE_INFLIGHT_ACC_CAL)) { if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > mcfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement InflightcalibratingA = 50; - AccInflightCalibrationArmed = 0; + AccInflightCalibrationArmed = false; } 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; } else if (AccInflightCalibrationMeasurementDone && !f.ARMED) { - AccInflightCalibrationMeasurementDone = 0; - AccInflightCalibrationSavetoEEProm = 1; + AccInflightCalibrationMeasurementDone = false; + AccInflightCalibrationSavetoEEProm = true; } } diff --git a/src/rx_spektrum.c b/src/rx_spektrum.c index f5655dcefd..f99c109f8f 100644 --- a/src/rx_spektrum.c +++ b/src/rx_spektrum.c @@ -45,7 +45,7 @@ static void spektrumDataReceive(uint16_t c) { uint32_t spekTime; static uint32_t spekTimeLast, spekTimeInterval; - static uint8_t spekFramePosition; + static uint8_t spekFramePosition; spekDataIncoming = true; spekTime = micros(); diff --git a/src/sensors_acceleration.c b/src/sensors_acceleration.c index 7cd81bc4a5..27c71e170e 100644 --- a/src/sensors_acceleration.c +++ b/src/sensors_acceleration.c @@ -4,10 +4,10 @@ uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. extern uint16_t InflightcalibratingA; -extern int16_t AccInflightCalibrationArmed; -extern uint16_t AccInflightCalibrationMeasurementDone; -extern uint16_t AccInflightCalibrationSavetoEEProm; -extern uint16_t AccInflightCalibrationActive; +extern bool AccInflightCalibrationArmed; +extern bool AccInflightCalibrationMeasurementDone; +extern bool AccInflightCalibrationSavetoEEProm; +extern bool AccInflightCalibrationActive; void ACC_Common(void) { @@ -62,8 +62,8 @@ void ACC_Common(void) } // all values are measured if (InflightcalibratingA == 1) { - AccInflightCalibrationActive = 0; - AccInflightCalibrationMeasurementDone = 1; + AccInflightCalibrationActive = false; + AccInflightCalibrationMeasurementDone = true; toggleBeep = 2; // buzzer for indicatiing the end of calibration // recover saved values to maintain current flight behavior until new values are transferred mcfg.accZero[ROLL] = accZero_saved[ROLL]; @@ -75,8 +75,8 @@ void ACC_Common(void) InflightcalibratingA--; } // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration - if (AccInflightCalibrationSavetoEEProm == 1) { // the copter is landed, disarmed and the combo has been done again - AccInflightCalibrationSavetoEEProm = 0; + if (AccInflightCalibrationSavetoEEProm) { // the copter is landed, disarmed and the combo has been done again + AccInflightCalibrationSavetoEEProm = false; mcfg.accZero[ROLL] = b[ROLL] / 50; mcfg.accZero[PITCH] = b[PITCH] / 50; mcfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G diff --git a/stm32_flash.ld b/stm32_flash.ld index abd809b4cc..13bf7a0231 100644 --- a/stm32_flash.ld +++ b/stm32_flash.ld @@ -4,7 +4,7 @@ ** File : stm32_flash.ld ** ** Abstract : Linker script for STM32F103C8 Device with -** 64KByte FLASH, 20KByte RAM +** 128KByte FLASH, 20KByte RAM ** ***************************************************************************** */ @@ -13,7 +13,7 @@ ENTRY(Reset_Handler) /* Highest address of the user mode stack */ -_estack = 0x20005000; /* end of 10K RAM */ +_estack = 0x20005000; /* end of 20K RAM */ /* Generate a link error if heap and stack don't fit into RAM */ _Min_Heap_Size = 0; /* required amount of heap */