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( configureIMU(
&imuRuntimeConfig, &imuRuntimeConfig,
&currentProfile->pidProfile, &currentProfile->pidProfile,
&currentProfile->accDeadband &currentProfile->accDeadband,
currentProfile->accz_lpf_cutoff,
currentProfile->throttle_correction_angle
); );
configureAltitudeHold( configureAltitudeHold(
@ -626,9 +628,6 @@ void activateConfig(void)
&masterConfig.escAndServoConfig &masterConfig.escAndServoConfig
); );
calculateThrottleAngleScale(currentProfile->throttle_correction_angle);
calculateAccZLowPassFilterRCTimeConstant(currentProfile->accz_lpf_cutoff);
#ifdef BARO #ifdef BARO
useBarometerConfig(&currentProfile->barometerConfig); useBarometerConfig(&currentProfile->barometerConfig);
#endif #endif

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

@ -79,11 +79,19 @@ imuRuntimeConfig_t *imuRuntimeConfig;
pidProfile_t *pidProfile; pidProfile_t *pidProfile;
accDeadband_t *accDeadband; 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; imuRuntimeConfig = initialImuRuntimeConfig;
pidProfile = initialPidProfile; pidProfile = initialPidProfile;
accDeadband = initialAccDeadband; accDeadband = initialAccDeadband;
fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff);
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
} }
void initIMU() void initIMU()
@ -93,14 +101,17 @@ void initIMU()
gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f; 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) 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; int8_t small_angle;
} imuRuntimeConfig_t; } 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 calculateEstimatedAltitude(uint32_t currentTime);
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode); 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); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
void accSum_reset(void); void accSum_reset(void);