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

Add roll pitch velocity

Browse source
This commit is contained in:
borisbstyle 2016-08-05 00:55:12 +02:00
parent 06be182e50
commit 38e812a5a5
5 changed files with 17 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -133,7 +133,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
float errorRate = 0, rP = 0, rD = 0, PVRate = 0;
float ITerm,PTerm,DTerm;
static float lastRateError[2];
static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate;
static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3];
float delta;
int axis;
float horizonLevelStrength = 1;
@ -188,6 +188,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
b[axis] = pidProfile->ptermSetpointWeight / 100.0f;
c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT();
rollPitchMaxVelocity = pidProfile->pidMaxVelocityRollPitch * 1000 * getdT();
configP[axis] = pidProfile->P8[axis];
configI[axis] = pidProfile->I8[axis];
@ -195,12 +196,13 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
}
// Limit abrupt yaw inputs / stops
if (axis == YAW && pidProfile->pidMaxVelocityYaw) {
float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate;
if (ABS(yawCurrentVelocity) > yawMaxVelocity) {
setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity;
float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
if (maxVelocity) {
float currentVelocity = setpointRate[axis] - previousSetpoint[axis];
if (ABS(currentVelocity) > maxVelocity) {
setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;
}
yawPreviousRate = setpointRate[axis];
previousSetpoint[axis] = setpointRate[axis];
}
// Yaw control is GYRO based, direct sticks control is applied to rate PID