diff --git a/src/main/config/config.c b/src/main/config/config.c index 17c656ddd9..010e686b08 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -616,7 +616,9 @@ void activateConfig(void) configureIMU( &imuRuntimeConfig, ¤tProfile->pidProfile, - ¤tProfile->accDeadband + ¤tProfile->accDeadband, + currentProfile->accz_lpf_cutoff, + currentProfile->throttle_correction_angle ); configureAltitudeHold( @@ -626,9 +628,6 @@ void activateConfig(void) &masterConfig.escAndServoConfig ); - calculateThrottleAngleScale(currentProfile->throttle_correction_angle); - calculateAccZLowPassFilterRCTimeConstant(currentProfile->accz_lpf_cutoff); - #ifdef BARO useBarometerConfig(¤tProfile->barometerConfig); #endif diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index f0a4535a00..284d11ab0e 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -79,11 +79,19 @@ imuRuntimeConfig_t *imuRuntimeConfig; pidProfile_t *pidProfile; accDeadband_t *accDeadband; -void configureIMU(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband) +void configureIMU( + imuRuntimeConfig_t *initialImuRuntimeConfig, + pidProfile_t *initialPidProfile, + accDeadband_t *initialAccDeadband, + float accz_lpf_cutoff, + uint16_t throttle_correction_angle +) { imuRuntimeConfig = initialImuRuntimeConfig; pidProfile = initialPidProfile; accDeadband = initialAccDeadband; + fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff); + throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); } void initIMU() @@ -93,14 +101,17 @@ void initIMU() gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f; } -void calculateThrottleAngleScale(uint16_t throttle_correction_angle) +float calculateThrottleAngleScale(uint16_t throttle_correction_angle) { - throttleAngleScale = (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); + return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); } -void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) +/* +* Calculate RC time constant used in the accZ lpf. +*/ +float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) { - fc_acc = 0.5f / (M_PIf * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf + return 0.5f / (M_PIf * accz_lpf_cutoff); } void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 552093508d..cf03c8e7f0 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -30,12 +30,18 @@ typedef struct imuRuntimeConfig_s { int8_t small_angle; } imuRuntimeConfig_t; -void configureIMU(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband); +void configureIMU( + imuRuntimeConfig_t *initialImuRuntimeConfig, + pidProfile_t *initialPidProfile, + accDeadband_t *initialAccDeadband, + float accz_lpf_cutoff, + uint16_t throttle_correction_angle +); void calculateEstimatedAltitude(uint32_t currentTime); void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode); -void calculateThrottleAngleScale(uint16_t throttle_correction_angle); +float calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); -void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); +float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); void accSum_reset(void);