rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Improved efficiency of calculation of horizonLevelStrength.

This commit is contained in:
Martin Budden 2015-12-29 06:48:30 +00:00 committed by borisbstyle
parent 5de4d1e489
commit dd5f6cc997
1 changed files with 7 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/main/flight/pid.c
View File

@ -102,7 +102,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
{ {
float RateError, AngleRate, gyroRate; float RateError, AngleRate, gyroRate;
float ITerm,PTerm,DTerm; float ITerm,PTerm,DTerm;
int32_t stickPosAil, stickPosEle, mostDeflectedPos;
static float lastError[3]; static float lastError[3];
static float previousDelta[3][8]; static float previousDelta[3][8];
float delta, deltaSum; float delta, deltaSum;
@ -113,18 +112,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
if (!deltaTotalSamples) setPidDeltaSamples(); if (!deltaTotalSamples) setPidDeltaSamples();
if (FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions // Figure out the raw stick positions
stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc); const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc); const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
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 // 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 horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection
if(pidProfile->H_sensitivity == 0){ if(pidProfile->H_sensitivity == 0){
@ -249,26 +240,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int32_t AngleRateTmp, RateError; int32_t AngleRateTmp, RateError;
int8_t horizonLevelStrength = 100; int8_t horizonLevelStrength = 100;
int32_t stickPosAil, stickPosEle, mostDeflectedPos;
if (!deltaTotalSamples) setPidDeltaSamples(); if (!deltaTotalSamples) setPidDeltaSamples();
if (FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions // Figure out the raw stick positions
stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc); const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc); const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
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 // Progressively turn off the horizon self level strength as the stick is banged over
horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection
// Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine.
// For more rate mode increase D and slower flips and rolls will be possible // For more rate mode increase D and slower flips and rolls will be possible
horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100);