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

new Baseflight PID

Browse source 
full gyro scale is used now
and a new pid with float calculations was added based on PIDrewrite
eeprom size was also increased from 1kB to 2kB

Conflicts:

	src/config.c
	src/drivers/accgyro_l3g4200d.c
	src/drivers/accgyro_mpu3050.c
	src/drivers/accgyro_mpu6050.c
	src/flight_imu.c
	src/mw.c
	src/mw.h
	src/serial_cli.c
	src/serial_msp.c
	src/utils.c
	src/utils.h
This commit is contained in:
Lukas S 2013-11-03 03:40:16 +01:00 • committed by Dominic Clifton
parent 1df79e65fc
commit cffdfb782c
15 changed files with 222 additions and 39 deletions
Download patch file Download diff file Expand all files Collapse all files

90
src/flight_common.c
View file

@ -7,6 +7,9 @@
#include "runtime_config.h"
#include "drivers/accgyro_common.h"
#include "sensors_common.h"
#include "sensors_gyro.h"
#include "rc_controls.h"
#include "flight_common.h"
#include "gps_common.h"
@ -18,6 +21,7 @@ int16_t axisPID[3];
uint8_t dynP8[3], dynI8[3], dynD8[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
static int32_t errorAngleI[2] = { 0, 0 };
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
@ -45,8 +49,80 @@ void resetErrorGyro(void)
errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
errorGyroIf[ROLL] = 0;
errorGyroIf[PITCH] = 0;
errorGyroIf[YAW] = 0;
}
static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
{
float RateError, errorAngle, AngleRate, gyroRate;
float ITerm,PTerm,DTerm;
static float lastGyroRate[3];
static float delta1[3], delta2[3];
float delta, deltaSum;
float dT;
int axis;
dT = (float)cycleTime * 0.000001f;
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode
if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
AngleRate = (float)((controlRateConfig->yawRate + 10) * rcCommand[YAW]) / 50.0f;
} else {
// calculate error and limit the angle to 50 degrees max inclination
errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -500, +500) - inclination.rawAngles[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate
if (f.HORIZON_MODE) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRate += errorAngle * pidProfile->H_level;
}
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * pidProfile->A_level;
}
}
gyroRate = gyroData[axis] * gyro.scale; // gyro output scaled to dps
// --------low-level gyro-based PID. ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// -----calculate scaled error.AngleRates
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRate - gyroRate;
// -----calculate P component
PTerm = RateError * pidProfile->P_f[axis];
// -----calculate I component
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis], -250.0f, 250.0f);
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
ITerm = errorGyroIf[axis];
//-----calculate D-term
delta = gyroRate - lastGyroRate[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyroRate[axis] = gyroRate;
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta *= (1.0f / dT);
// add moving average here to reduce noise
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis], -300.0f, 300.0f);
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000);
}
}
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
{
@ -73,14 +149,14 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
}
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
error -= gyroData[axis];
error -= gyroData[axis] / 4;
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (abs(gyroData[axis]) > 640)
if (abs(gyroData[axis]) > (640 * 4))
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) >> 6;
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64;
}
if (f.HORIZON_MODE && (axis == FD_ROLL || axis == FD_PITCH)) {
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
@ -95,8 +171,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
}
}
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis];
PTerm -= ((int32_t)gyroData[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = (gyroData[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
@ -144,7 +220,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// -----calculate scaled error.AngleRates
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRateTmp - gyroData[axis];
RateError = AngleRateTmp - (gyroData[axis] / 4);
// -----calculate P component
PTerm = (RateError * pidProfile->P8[axis]) >> 7;
@ -188,6 +264,8 @@ void setPIDController(int type)
case 1:
pid_controller = pidRewrite;
break;
case 2:
pid_controller = pidBaseflight;
}
}