diff --git a/src/cli.c b/src/cli.c index f42e25a6a4..40457bc868 100644 --- a/src/cli.c +++ b/src/cli.c @@ -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, 1 }, + { "pid_controller", VAR_UINT8, &cfg.pidController, 0, 2 }, { "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,6 +201,17 @@ 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 }, diff --git a/src/config.c b/src/config.c index 7e715675a6..7b42523c65 100755 --- a/src/config.c +++ b/src/config.c @@ -7,7 +7,7 @@ #endif #define FLASH_PAGE_SIZE ((uint16_t)0x400) -#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 1)) // use the last KB for storage +#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 2)) // use the last 2 KB for storage master_t mcfg; // master config struct with data independent from profiles config_t cfg; // profile config struct @@ -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) { + if (FLASH_ErasePage(FLASH_WRITE_ADDR) == FLASH_COMPLETE && FLASH_ErasePage(FLASH_WRITE_ADDR + FLASH_PAGE_SIZE) == 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,6 +246,19 @@ 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; diff --git a/src/drv_l3g4200d.c b/src/drv_l3g4200d.c index 4c8e2af62e..658acd6621 100644 --- a/src/drv_l3g4200d.c +++ b/src/drv_l3g4200d.c @@ -44,7 +44,7 @@ bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf) gyro->read = l3g4200dRead; // 14.2857dps/lsb scalefactor - gyro->scale = (((32767.0f / 14.2857f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f)); + gyro->scale = 1.0f / 14.2857f; // 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]) / 4; - data[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4; - data[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4; + 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]); alignSensors(data, gyroData, gyroAlign); } diff --git a/src/drv_mpu3050.c b/src/drv_mpu3050.c index 133ab7b10b..5ac3d4623b 100755 --- a/src/drv_mpu3050.c +++ b/src/drv_mpu3050.c @@ -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 = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f)); + gyro->scale = 1.0f / 16.4f; // 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]) / 4; - data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4; - data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4; + 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]); alignSensors(data, gyroData, gyroAlign); } diff --git a/src/drv_mpu6050.c b/src/drv_mpu6050.c index 94bfc79d82..1637afb7c9 100644 --- a/src/drv_mpu6050.c +++ b/src/drv_mpu6050.c @@ -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 = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f; + gyro->scale = 1.0f / 16.4f; // 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]) / 4; - data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4; - data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4; + 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]); alignSensors(data, gyroData, gyroAlign); } diff --git a/src/imu.c b/src/imu.c index d4a7380065..ad64da3e84 100755 --- a/src/imu.c +++ b/src/imu.c @@ -19,6 +19,7 @@ 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 @@ -34,6 +35,7 @@ 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 @@ -261,7 +263,7 @@ static void getEstimatedAttitude(void) uint32_t deltaT; float scale, deltaGyroAngle[3]; deltaT = currentT - previousT; - scale = deltaT * gyro.scale; + scale = deltaT * gyroScaleRad; previousT = currentT; // Initialization diff --git a/src/mw.c b/src/mw.c index 9687884768..551c467421 100755 --- a/src/mw.c +++ b/src/mw.c @@ -25,6 +25,7 @@ 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]; @@ -278,8 +279,78 @@ 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; @@ -304,14 +375,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]; + 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]) > 2560) errorGyroI[axis] = 0; - ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; + ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) / 64; } if (f.HORIZON_MODE && axis < 2) { PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500; @@ -326,8 +397,8 @@ static void pidMultiWii(void) } } - 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]; @@ -374,7 +445,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]; + RateError = AngleRateTmp - (gyroData[axis] / 4); // -----calculate P component PTerm = (RateError * cfg.P8[axis]) >> 7; @@ -418,6 +489,8 @@ void setPIDController(int type) case 1: pid_controller = pidRewrite; break; + case 2: + pid_controller = pidBaseflight; } } @@ -514,6 +587,9 @@ 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 diff --git a/src/mw.h b/src/mw.h index f9efeee581..59cc547295 100755 --- a/src/mw.h +++ b/src/mw.h @@ -150,10 +150,16 @@ 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 + 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 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; diff --git a/src/serial.c b/src/serial.c index 001ac6f178..13182400cc 100755 --- a/src/serial.c +++ b/src/serial.c @@ -296,10 +296,31 @@ static void evaluateCommand(void) headSerialReply(0); break; case MSP_SET_PID: - for (i = 0; i < PIDITEMS; i++) { - cfg.P8[i] = read8(); - cfg.I8[i] = read8(); - cfg.D8[i] = read8(); + 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(); + } } headSerialReply(0); break; @@ -477,10 +498,31 @@ static void evaluateCommand(void) break; case MSP_PID: headSerialReply(3 * PIDITEMS); - for (i = 0; i < PIDITEMS; i++) { - serialize8(cfg.P8[i]); - serialize8(cfg.I8[i]); - serialize8(cfg.D8[i]); + 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]); + } } break; case MSP_PIDNAMES: diff --git a/src/utils.c b/src/utils.c index 0b31b002e5..d4873cb42b 100644 --- a/src/utils.c +++ b/src/utils.c @@ -14,6 +14,16 @@ 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; diff --git a/src/utils.h b/src/utils.h index c6ef6b8cbb..934dfe369b 100644 --- a/src/utils.h +++ b/src/utils.h @@ -1,6 +1,7 @@ #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);