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

Merge pull request #101 from luggi/althold-flipable 

make flipping the quad safe with althold engaged
This commit is contained in:
dongie 2014-05-04 22:50:50 +09:00
parent c1fcdabdc1 495c6b8f73
commit ac4835ef67
2 changed files with 23 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/imu.c
View File

@ -391,24 +391,30 @@ int getEstimatedAltitude(void)
// set vario
vario = applyDeadband(vel_tmp, 5);
if (abs(angle[ROLL]) < 800 && abs(angle[PITCH]) < 800) { // only calculate pid if the copters thrust is facing downwards(<80deg)
// Altitude P-Controller
error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
setVel = constrain((cfg.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
// Altitude P-Controller
error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
setVel = constrain((cfg.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
// Velocity PID-Controller
// P
error = setVel - vel_tmp;
BaroPID = constrain((cfg.P8[PIDVEL] * error / 32), -300, +300);
// Velocity PID-Controller
// P
error = setVel - vel_tmp;
BaroPID = constrain((cfg.P8[PIDVEL] * error / 32), -300, +300);
// I
errorAltitudeI += (cfg.I8[PIDVEL] * error) / 8;
errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200));
BaroPID += errorAltitudeI / 1024; // I in range +/-200
// I
errorAltitudeI += (cfg.I8[PIDVEL] * error) / 8;
errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200));
BaroPID += errorAltitudeI / 1024; // I in range +/-200
// D
BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
// D
BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
} else {
BaroPID = 0;
}
accZ_old = accZ_tmp;
return 1;

5
src/mw.c
View File

@ -854,7 +854,7 @@ void loop(void)
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, mcfg.minthrottle + 100, mcfg.maxthrottle);
}
} else {
// slow alt changes for apfags
@ -869,8 +869,7 @@ void loop(void)
AltHoldCorr = 0;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle);
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, mcfg.minthrottle + 100, mcfg.maxthrottle);
}
} else {
// handle fixedwing-related althold. UNTESTED! and probably wrong