diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 63158f451c..962d9e562a 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -298,11 +298,13 @@ static void imuCalculateEstimatedAttitude(void) imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame } -void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) +void imuUpdate(rollAndPitchTrims_t *accelerometerTrims) { - static int16_t gyroYawSmooth = 0; - gyroUpdate(); + gyroData[FD_ROLL] = gyroADC[FD_ROLL]; + gyroData[FD_PITCH] = gyroADC[FD_PITCH]; + gyroData[FD_YAW] = gyroADC[FD_YAW]; + if (sensors(SENSOR_ACC)) { updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer' imuCalculateEstimatedAttitude(); @@ -311,16 +313,6 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) accADC[Y] = 0; accADC[Z] = 0; } - - gyroData[FD_ROLL] = gyroADC[FD_ROLL]; - gyroData[FD_PITCH] = gyroADC[FD_PITCH]; - - if (mixerMode == MIXER_TRI) { - gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3; - gyroYawSmooth = gyroData[FD_YAW]; - } else { - gyroData[FD_YAW] = gyroADC[FD_YAW]; - } } int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index afd34566d4..6cb95d7c0d 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -62,7 +62,7 @@ void imuConfigure( ); void calculateEstimatedAltitude(uint32_t currentTime); -void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode); +void imuUpdate(rollAndPitchTrims_t *accelerometerTrims); float calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); diff --git a/src/main/mw.c b/src/main/mw.c index e00de99408..111841cbce 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -720,7 +720,7 @@ void loop(void) if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { loopTime = currentTime + masterConfig.looptime; - imuUpdate(¤tProfile->accelerometerTrims, masterConfig.mixerMode); + imuUpdate(¤tProfile->accelerometerTrims); // Measure loop rate just after reading the sensors currentTime = micros();