Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

Fix level mode in betaflight PID controller

Browse source
This commit is contained in:
borisbstyle 2016-09-10 23:29:48 +02:00
parent 4a573d65c9
commit cb16d65532
2 changed files with 3 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -218,11 +218,11 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;
ptermSetpointRate[axis] = setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;
} else {
// HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel
setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f);
ptermSetpointRate[axis] = setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f);
}
}

2
src/main/mw.c
View file

@ -203,7 +203,7 @@ void calculateSetpointRate(int axis, int16_t rc) {
rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) {
ptermSetpointRate[axis] = constrainf(angleRate * rcSuperfactor, -1998.0f, 1998.0f);
if (currentProfile->pidProfile.ptermSRateWeight < 100 && axis != YAW) {
if (currentProfile->pidProfile.ptermSRateWeight < 100 && axis != YAW && !flightModeFlags) {
const float pWeight = currentProfile->pidProfile.ptermSRateWeight / 100.0f;
angleRate = angleRate + (pWeight * ptermSetpointRate[axis] - angleRate);
} else {