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

Cleanup mw.c // Remove unnecessary functions

Browse source
This commit is contained in:
borisbstyle 2016-12-18 01:28:20 +01:00
parent 979096f333
commit 4e3704374a
17 changed files with 46 additions and 348 deletions
Download patch file Download diff file Expand all files Collapse all files

28
src/main/flight/pid.c
View file

@ -34,7 +34,6 @@
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/navigation.h"
#include "flight/gtune.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
@ -183,24 +182,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
}
}
// Yet Highly experimental and under test and development
// Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols)
static float kiThrottleGain = 1.0f;
if (pidProfile->itermThrottleGain) {
const uint16_t maxLoopCount = 20000 / targetPidLooptime;
const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f;
static int16_t previousThrottle;
static uint16_t loopIncrement;
if (loopIncrement >= maxLoopCount) {
kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5
previousThrottle = rcCommand[THROTTLE];
loopIncrement = 0;
} else {
loopIncrement++;
}
}
// ----------PID controller----------
const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -247,10 +228,9 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
// Reduce strong Iterm accumulation during higher stick inputs
const float accumulationThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
const float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f);
const float itermScaler = setpointRateScaler * kiThrottleGain;
float ITerm = previousGyroIf[axis];
ITerm += Ki[axis] * errorRate * dT * itermScaler;;
ITerm += Ki[axis] * errorRate * dT * setpointRateScaler;
// limit maximum integrator value to prevent WindUp
ITerm = constrainf(ITerm, -250.0f, 250.0f);
previousGyroIf[axis] = ITerm;
@ -291,12 +271,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
// Disable PID control at zero throttle
if (!pidStabilisationEnabled) axisPIDf[axis] = 0;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;