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

Add PID config initialisation

This commit is contained in:
borisbstyle 2016-11-30 12:52:43 +01:00
parent 84b79b1789
commit 4a0f678dec
4 changed files with 23 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/fc/fc_msp.c
View File

@ -1264,6 +1264,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
currentProfile->pidProfile.I8[i] = sbufReadU8(src);
currentProfile->pidProfile.D8[i] = sbufReadU8(src);
}
pidInitConfig(&currentProfile->pidProfile);
break;
case MSP_SET_MODE_RANGE:
@ -1478,6 +1479,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
currentProfile->pidProfile.itermThrottleGain = sbufReadU8(src);
currentProfile->pidProfile.rateAccelLimit = sbufReadU16(src);
currentProfile->pidProfile.yawRateAccelLimit = sbufReadU16(src);
pidInitConfig(&currentProfile->pidProfile);
break;
case MSP_SET_SENSOR_CONFIG:

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

@ -56,9 +56,12 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
static float errorGyroIf[3];
static float dT;
void pidSetTargetLooptime(uint32_t pidLooptime)
{
targetPidLooptime = pidLooptime;
dT = (float)targetPidLooptime * 0.000001f;
}
void pidResetErrorGyroState(void)
@ -91,7 +94,6 @@ void pidInitFilters(const pidProfile_t *pidProfile)
static pt1Filter_t pt1FilterYaw;
BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
const float dT = (float)targetPidLooptime * 0.000001f;
if (pidProfile->dterm_notch_hz == 0) {
dtermNotchFilterApplyFn = nullFilterApply;
@ -144,20 +146,28 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
static float Kp[3], Ki[3], Kd[3], c[3];
static float rollPitchMaxVelocity, yawMaxVelocity, relaxFactor[3];
void pidInitConfig(const pidProfile_t *pidProfile) {
for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f);
}
yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * dT;
rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT;
}
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)
void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, uint16_t midrc)
{
static float lastRateError[2];
static float Kp[3], Ki[3], Kd[3], c[3];
static float rollPitchMaxVelocity, yawMaxVelocity;
static float previousSetpoint[3], relaxFactor[3];
static float dT;
if (!dT) {
dT = (float)targetPidLooptime * 0.000001f;
}
static float previousSetpoint[3];
float horizonLevelStrength = 1;
if (FLIGHT_MODE(HORIZON_MODE)) {
@ -195,25 +205,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
// ----------PID controller----------
const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
static uint8_t configP[3], configI[3], configD[3];
// Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now
// Prepare all parameters for PID controller
if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) {
Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f);
yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * dT;
rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT;
configP[axis] = pidProfile->P8[axis];
configI[axis] = pidProfile->I8[axis];
configD[axis] = pidProfile->D8[axis];
}
// Limit abrupt yaw inputs / stops
const float maxVelocity = (axis == FD_YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
if (maxVelocity) {

1
src/main/flight/pid.h
View File

@ -106,4 +106,5 @@ void pidResetErrorGyroState(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState);
void pidSetTargetLooptime(uint32_t pidLooptime);
void pidInitFilters(const pidProfile_t *pidProfile);
void pidInitConfig(const pidProfile_t *pidProfile);

1
src/main/main.c
View File

@ -451,6 +451,7 @@ void init(void)
// gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidSetTargetLooptime()
pidSetTargetLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
pidInitFilters(&currentProfile->pidProfile);
pidInitConfig(&currentProfile->pidProfile);
imuInit();