Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00
Code Issues Wiki Activity

Moved configuring of IMU all into one function call.

Browse source
This commit is contained in:
Pierre Hugo 2015-01-23 23:03:22 -08:00
parent be03ed95fa
commit d691f72849
3 changed files with 28 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

7
src/main/config/config.c
View file

@ -616,7 +616,9 @@ void activateConfig(void)
configureIMU(
&imuRuntimeConfig,
&currentProfile->pidProfile,
&currentProfile->accDeadband
&currentProfile->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(&currentProfile->barometerConfig);
#endif

21
src/main/flight/imu.c
View file

@ -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)

12
src/main/flight/imu.h
View file

@ -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);