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

Revert new PID stuff and move it to a separate branch for now.

This commit is contained in:
fiendie 2013-11-07 13:15:31 +01:00
parent 14f087a140
commit 6ab48fc438
11 changed files with 30 additions and 187 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/cli.c
View File

@ -147,7 +147,7 @@ const clivalue_t valueTable[] = {
{ "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 },
{ "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 },
{ "gyro_cmpfm_factor", VAR_UINT16, &mcfg.gyro_cmpfm_factor, 100, 1000 },
{ "pid_controller", VAR_UINT8, &cfg.pidController, 0, 2 },
{ "pid_controller", VAR_UINT8, &cfg.pidController, 0, 1 },
{ "deadband", VAR_UINT8, &cfg.deadband, 0, 32 },
{ "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 },
{ "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 },
@ -201,17 +201,6 @@ const clivalue_t valueTable[] = {
{ "p_yaw", VAR_UINT8, &cfg.P8[YAW], 0, 200 },
{ "i_yaw", VAR_UINT8, &cfg.I8[YAW], 0, 200 },
{ "d_yaw", VAR_UINT8, &cfg.D8[YAW], 0, 200 },
{ "Ppitchf", VAR_FLOAT, &cfg.P_f[PITCH], 0, 100 },
{ "Ipitchf", VAR_FLOAT, &cfg.I_f[PITCH], 0, 100 },
{ "Dpitchf", VAR_FLOAT, &cfg.D_f[PITCH], 0, 100 },
{ "Prollf", VAR_FLOAT, &cfg.P_f[ROLL], 0, 100 },
{ "Irollf", VAR_FLOAT, &cfg.I_f[ROLL], 0, 100 },
{ "Drollf", VAR_FLOAT, &cfg.D_f[ROLL], 0, 100 },
{ "Pyawf", VAR_FLOAT, &cfg.P_f[YAW], 0, 100 },
{ "Iyawf", VAR_FLOAT, &cfg.I_f[YAW], 0, 100 },
{ "Dyawf", VAR_FLOAT, &cfg.D_f[YAW], 0, 100 },
{ "level_horizon", VAR_FLOAT, &cfg.H_level, 0, 10 },
{ "level_angle", VAR_FLOAT, &cfg.A_level, 0, 10 },
{ "p_alt", VAR_UINT8, &cfg.P8[PIDALT], 0, 200 },
{ "i_alt", VAR_UINT8, &cfg.I8[PIDALT], 0, 200 },
{ "d_alt", VAR_UINT8, &cfg.D8[PIDALT], 0, 200 },

15
src/config.c
View File

@ -118,7 +118,7 @@ retry:
FLASH_Unlock();
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
if (FLASH_ErasePage(FLASH_WRITE_ADDR) == FLASH_COMPLETE && FLASH_ErasePage(FLASH_WRITE_ADDR + FLASH_PAGE_SIZE) == FLASH_COMPLETE) {
if (FLASH_ErasePage(FLASH_WRITE_ADDR) == FLASH_COMPLETE) {
for (i = 0; i < sizeof(master_t); i += 4) {
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *) ((char *)&mcfg + i));
if (status != FLASH_COMPLETE) {
@ -246,19 +246,6 @@ static void resetConf(void)
cfg.P8[PIDVEL] = 0;
cfg.I8[PIDVEL] = 0;
cfg.D8[PIDVEL] = 0;
cfg.P_f[ROLL] = 2.5f; // new PID with preliminary defaults test carefully
cfg.I_f[ROLL] = 0.3f;
cfg.D_f[ROLL] = 0.06f;
cfg.P_f[PITCH] = 2.5f;
cfg.I_f[PITCH] = 0.3f;
cfg.D_f[PITCH] = 0.06f;
cfg.P_f[YAW] = 8.0f;
cfg.I_f[YAW] = 0.5f;
cfg.D_f[YAW] = 0.05f;
cfg.A_level = 5.0f;
cfg.H_level = 3.0f;
cfg.rcRate8 = 90;
cfg.rcExpo8 = 65;
cfg.rollPitchRate = 0;

8
src/drv_l3g4200d.c
View File

@ -44,7 +44,7 @@ bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
gyro->read = l3g4200dRead;
// 14.2857dps/lsb scalefactor
gyro->scale = 1.0f / 14.2857f;
gyro->scale = (((32767.0f / 14.2857f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
// default LPF is set to 32Hz
switch (lpf) {
@ -89,9 +89,9 @@ static void l3g4200dRead(int16_t *gyroData)
int16_t data[3];
i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf);
data[X] = (int16_t)((buf[0] << 8) | buf[1]);
data[Y] = (int16_t)((buf[2] << 8) | buf[3]);
data[Z] = (int16_t)((buf[4] << 8) | buf[5]);
data[X] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}

8
src/drv_mpu3050.c
View File

@ -45,7 +45,7 @@ bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
gyro->read = mpu3050Read;
gyro->temperature = mpu3050ReadTemp;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
// default lpf is 42Hz
switch (lpf) {
@ -99,9 +99,9 @@ static void mpu3050Read(int16_t *gyroData)
int16_t data[3];
i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
data[0] = (int16_t)((buf[0] << 8) | buf[1]);
data[1] = (int16_t)((buf[2] << 8) | buf[3]);
data[2] = (int16_t)((buf[4] << 8) | buf[5]);
data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}

8
src/drv_mpu6050.c
View File

@ -167,7 +167,7 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
gyro->read = mpu6050GyroRead;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
// give halfacc (old revision) back to system
if (scale)
@ -261,9 +261,9 @@ static void mpu6050GyroRead(int16_t *gyroData)
int16_t data[3];
i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
data[0] = (int16_t)((buf[0] << 8) | buf[1]);
data[1] = (int16_t)((buf[2] << 8) | buf[3]);
data[2] = (int16_t)((buf[4] << 8) | buf[5]);
data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}

4
src/imu.c
View File

@ -19,7 +19,6 @@ int32_t vario = 0; // variometer in cm/s
int16_t throttleAngleCorrection = 0; // correction of throttle in lateral wind,
float magneticDeclination = 0.0f; // calculated at startup from config
float accVelScale;
float gyroScaleRad;
// **************
// gyro+acc IMU
@ -35,7 +34,6 @@ void imuInit(void)
{
accZ_25deg = acc_1G * cosf(RAD * 25.0f);
accVelScale = 9.80665f / acc_1G / 10000.0f;
gyroScaleRad = gyro.scale * (M_PI / 180.0f) * 0.000001f;
#ifdef MAG
// if mag sensor is enabled, use it
@ -263,7 +261,7 @@ static void getEstimatedAttitude(void)
uint32_t deltaT;
float scale, deltaGyroAngle[3];
deltaT = currentT - previousT;
scale = deltaT * gyroScaleRad;
scale = deltaT * gyro.scale;
previousT = currentT;
// Initialization

88
src/mw.c
View File

@ -25,7 +25,6 @@ rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) o
static void pidMultiWii(void);
static void pidRewrite(void);
static void pidBaseflight(void);
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
uint8_t dynP8[3], dynI8[3], dynD8[3];
@ -279,78 +278,8 @@ static void mwVario(void)
}
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 pidBaseflight()
{
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 ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2 ) { // MODE relying on ACC
// calculate error and limit the angle to 50 degrees max inclination
errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]) / 10.0f; // 16 bits is ok here
}
if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
AngleRate = (float)((cfg.yawRate + 10) * rcCommand[YAW]) / 50.0f;
} else {
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRate = (float)((cfg.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 * cfg.H_level;
}
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * cfg.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 * cfg.P_f[axis];
// -----calculate I component
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * cfg.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) * cfg.D_f[axis], -300.0f, 300.0f);
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000);
}
}
static void pidMultiWii(void)
{
int axis, prop;
@ -375,14 +304,14 @@ static void pidMultiWii(void)
}
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis];
error -= gyroData[axis] / 4;
error -= gyroData[axis];
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (abs(gyroData[axis]) > 2560)
if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) / 64;
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
}
if (f.HORIZON_MODE && axis < 2) {
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
@ -397,8 +326,8 @@ static void pidMultiWii(void)
}
}
PTerm -= ((int32_t)gyroData[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = (gyroData[axis] - lastGyro[axis]) / 4;
PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis];
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
@ -445,7 +374,7 @@ static void pidRewrite(void)
// 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] / 4);
RateError = AngleRateTmp - gyroData[axis];
// -----calculate P component
PTerm = (RateError * cfg.P8[axis]) >> 7;
@ -489,8 +418,6 @@ void setPIDController(int type)
case 1:
pid_controller = pidRewrite;
break;
case 2:
pid_controller = pidBaseflight;
}
}
@ -587,9 +514,6 @@ void loop(void)
errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
errorGyroIf[ROLL] = 0;
errorGyroIf[PITCH] = 0;
errorGyroIf[YAW] = 0;
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
if (cfg.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX

8
src/mw.h
View File

@ -150,16 +150,10 @@ enum {
#define CALIBRATING_BARO_CYCLES 200
typedef struct config_t {
uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Luggi09s new baseflight pid
uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671
uint8_t P8[PIDITEMS];
uint8_t I8[PIDITEMS];
uint8_t D8[PIDITEMS];
float P_f[3]; // float p i and d factors for the new baseflight pid
float I_f[3];
float D_f[3];
float A_level;
float H_level;
uint8_t rcRate8;
uint8_t rcExpo8;

54
src/serial.c
View File

@ -296,29 +296,10 @@ static void evaluateCommand(void)
headSerialReply(0);
break;
case MSP_SET_PID:
if (cfg.pidController == 2) {
for (i = 0; i < 3; i++) {
cfg.P_f[i] = (float)read8() / 10.0f;
cfg.I_f[i] = (float)read8() / 100.0f;
cfg.D_f[i] = (float)read8() / 1000.0f;
}
for (i = 3; i < PIDITEMS; i++) {
if (i == PIDLEVEL) {
cfg.A_level = (float)read8() / 10.0f;
cfg.H_level = (float)read8() / 10.0f;
read8();
} else {
cfg.P8[i] = read8();
cfg.I8[i] = read8();
cfg.D8[i] = read8();
}
}
} else {
for (i = 0; i < PIDITEMS; i++) {
cfg.P8[i] = read8();
cfg.I8[i] = read8();
cfg.D8[i] = read8();
}
for (i = 0; i < PIDITEMS; i++) {
cfg.P8[i] = read8();
cfg.I8[i] = read8();
cfg.D8[i] = read8();
}
headSerialReply(0);
break;
@ -496,29 +477,10 @@ static void evaluateCommand(void)
break;
case MSP_PID:
headSerialReply(3 * PIDITEMS);
if (cfg.pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
for (i = 0; i < 3; i++) {
serialize8(constrain(lrintf(cfg.P_f[i] * 10.0f), 0, 250));
serialize8(constrain(lrintf(cfg.I_f[i] * 100.0f), 0, 250));
serialize8(constrain(lrintf(cfg.D_f[i] * 1000.0f), 0, 100));
}
for (i = 3; i < PIDITEMS; i++) {
if (i == PIDLEVEL) {
serialize8(constrain(lrintf(cfg.A_level * 10.0f), 0, 250));
serialize8(constrain(lrintf(cfg.H_level * 10.0f), 0, 250));
serialize8(0);
} else {
serialize8(cfg.P8[i]);
serialize8(cfg.I8[i]);
serialize8(cfg.D8[i]);
}
}
} else {
for (i = 0; i < PIDITEMS; i++) {
serialize8(cfg.P8[i]);
serialize8(cfg.I8[i]);
serialize8(cfg.D8[i]);
}
for (i = 0; i < PIDITEMS; i++) {
serialize8(cfg.P8[i]);
serialize8(cfg.I8[i]);
serialize8(cfg.D8[i]);
}
break;
case MSP_PIDNAMES:

10
src/utils.c
View File

@ -14,16 +14,6 @@ int constrain(int amt, int low, int high)
return amt;
}
float constrainf(float amt, float low, float high)
{
if (amt < low)
return low;
else if (amt > high)
return high;
else
return amt;
}
void initBoardAlignment(void)
{
float roll, pitch, yaw;

1
src/utils.h
View File

@ -1,7 +1,6 @@
#pragma once
int constrain(int amt, int low, int high);
float constrainf(float amt, float low, float high);
// sensor orientation
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation);
void initBoardAlignment(void);