Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 22:05:17 +03:00
Code Issues Wiki Activity

Decoupling imu from config - pid profile.

Browse source
This commit is contained in:
Dominic Clifton 2014-06-06 20:45:59 +01:00
parent da73be1b2d
commit 82bb6b7982
3 changed files with 9 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -389,7 +389,7 @@ void activateConfig(void)
imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor; imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor;
imuRuntimeConfig.acc_lpf_factor = currentProfile.acc_lpf_factor; imuRuntimeConfig.acc_lpf_factor = currentProfile.acc_lpf_factor;
configureImu(&imuRuntimeConfig); configureImu(&imuRuntimeConfig, &currentProfile.pidProfile);
calculateThrottleAngleScale(currentProfile.throttle_correction_angle); calculateThrottleAngleScale(currentProfile.throttle_correction_angle);

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

@ -105,10 +105,12 @@ void calculateThrottleAngleScale(uint16_t throttle_correction_angle)
} }
imuRuntimeConfig_t *imuRuntimeConfig; imuRuntimeConfig_t *imuRuntimeConfig;
pidProfile_t *pidProfile;
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig) void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile)
{ {
imuRuntimeConfig = initialImuRuntimeConfig; imuRuntimeConfig = initialImuRuntimeConfig;
pidProfile = initialPidProfile;
} }
void computeIMU(rollAndPitchTrims_t *accelerometerTrims) void computeIMU(rollAndPitchTrims_t *accelerometerTrims)
@ -399,21 +401,21 @@ int32_t calculateBaroPid(int32_t vel_tmp, float accZ_tmp, float accZ_old)
error = constrain(AltHold - EstAlt, -500, 500); error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
setVel = constrain((currentProfile.pidProfile.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
// Velocity PID-Controller // Velocity PID-Controller
// P // P
error = setVel - vel_tmp; error = setVel - vel_tmp;
baroPID = constrain((currentProfile.pidProfile.P8[PIDVEL] * error / 32), -300, +300); baroPID = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
// I // I
errorAltitudeI += (currentProfile.pidProfile.I8[PIDVEL] * error) / 8; errorAltitudeI += (pidProfile->I8[PIDVEL] * error) / 8;
errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200)); errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200));
baroPID += errorAltitudeI / 1024; // I in range +/-200 baroPID += errorAltitudeI / 1024; // I in range +/-200
// D // D
baroPID -= constrain(currentProfile.pidProfile.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150); baroPID -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
return baroPID; return baroPID;
} }

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

@ -27,7 +27,7 @@ typedef struct imuRuntimeConfig_s {
float gyro_cmpfm_factor; float gyro_cmpfm_factor;
} imuRuntimeConfig_t; } imuRuntimeConfig_t;
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig); void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile);
int getEstimatedAltitude(void); int getEstimatedAltitude(void);
void computeIMU(rollAndPitchTrims_t *accelerometerTrims); void computeIMU(rollAndPitchTrims_t *accelerometerTrims);