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

Merge pull request #351 from nebbian/baseflight-pid-horizon-mode-tuneup_a

Browse source 
Baseflight pid (pid_controller=2) horizon mode tuneup
This commit is contained in:
Dominic Clifton 2015-01-16 00:50:12 +00:00
commit b7462c0b3d
7 changed files with 37 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

26
src/main/flight/flight.c
View file

@ -101,14 +101,38 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
{
float RateError, errorAngle, AngleRate, gyroRate;
float ITerm,PTerm,DTerm;
int32_t stickPosAil, stickPosEle, mostDeflectedPos;
static float lastGyroRate[3];
static float delta1[3], delta2[3];
float delta, deltaSum;
float dT;
int axis;
float horizonLevelStrength = 1;
dT = (float)cycleTime * 0.000001f;
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
stickPosAil = getRcStickPosition(FD_ROLL);
stickPosEle = getRcStickPosition(FD_PITCH);
if(abs(stickPosAil) > abs(stickPosEle)){
mostDeflectedPos = abs(stickPosAil);
}
else {
mostDeflectedPos = abs(stickPosEle);
}
// Progressively turn off the horizon self level strength as the stick is banged over
horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection
if(pidProfile->H_sensitivity == 0){
horizonLevelStrength = 0;
} else {
horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1);
}
}
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode
@ -139,7 +163,7 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate
if (FLIGHT_MODE(HORIZON_MODE)) {
// mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->H_level;
AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength;
}
}
}