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

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, 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 },

src/config.c

@@ -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;

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 = 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[X] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
-    data[Y] = (int16_t)((buf[2] << 8) | buf[3]);
+    data[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
-    data[Z] = (int16_t)((buf[4] << 8) | buf[5]);
+    data[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
 
     alignSensors(data, gyroData, gyroAlign);
 }

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 = 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[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
-    data[1] = (int16_t)((buf[2] << 8) | buf[3]);
+    data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
-    data[2] = (int16_t)((buf[4] << 8) | buf[5]);
+    data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
 
     alignSensors(data, gyroData, gyroAlign);
 }

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 = 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[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
-    data[1] = (int16_t)((buf[2] << 8) | buf[3]);
+    data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
-    data[2] = (int16_t)((buf[4] << 8) | buf[5]);
+    data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
 
     alignSensors(data, gyroData, gyroAlign);
 }

src/imu.c

@@ -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

src/mw.c

@@ -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
+        PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
-        delta = (gyroData[axis] - lastGyro[axis]) / 4;
+        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

src/mw.h

@@ -150,17 +150,11 @@ 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;
     uint8_t thrMid8;

src/serial.c

@@ -296,29 +296,10 @@ static void evaluateCommand(void)
         headSerialReply(0);
         break;
     case MSP_SET_PID:
-        if (cfg.pidController == 2) {
+        for (i = 0; i < PIDITEMS; i++) {
-            for (i = 0; i < 3; i++) {
+            cfg.P8[i] = read8();
-                cfg.P_f[i] = (float)read8() / 10.0f;
+            cfg.I8[i] = read8();
-                cfg.I_f[i] = (float)read8() / 100.0f;
+            cfg.D8[i] = read8();
-                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;
@@ -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 < PIDITEMS; i++) {
-            for (i = 0; i < 3; i++) {
+            serialize8(cfg.P8[i]);
-                serialize8(constrain(lrintf(cfg.P_f[i] * 10.0f), 0, 250));
+            serialize8(cfg.I8[i]);
-                serialize8(constrain(lrintf(cfg.I_f[i] * 100.0f), 0, 250));
+            serialize8(cfg.D8[i]);
-                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:

src/utils.c

@@ -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;

src/utils.h

@@ -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);