Harakiri PID

src/cli.c

@@ -185,7 +185,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 },

src/drv_pwm.c

@@ -455,6 +455,7 @@ bool pwmInit(drv_pwm_config_t *init)
         pwmWritePtr = pwmWriteSyncPwm;
 
     // set return values in init struct
+    init->numMotors = numMotors;
     init->numServos = numServos;
 
     return false;

src/drv_pwm.h

@@ -31,6 +31,7 @@ typedef struct drv_pwm_config_t {
     bool fastPWM;
 
     // OUT parameters, filled by driver
+    uint8_t numMotors;
     uint8_t numServos;
 } drv_pwm_config_t;
 

src/main.c

@@ -180,6 +180,7 @@ int main(void)
     }
 
     pwmInit(&pwm_params);
+    core.numMotors = pwm_params.numMotors;
     core.numServos = pwm_params.numServos;
 
     // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled

src/mw.c

@@ -326,6 +326,8 @@ static void mwVario(void)
 
 static int32_t errorGyroI[3] = { 0, 0, 0 };
 static int32_t errorAngleI[2] = { 0, 0 };
+static float errorGyroIf[2] = { 0.0f, 0.0f };
+static float errorAngleIf[2] = { 0.0f, 0.0f };
 
 static void pidMultiWii(void)
 {
@@ -454,6 +456,112 @@ static void pidRewrite(void)
     }
 }
 
+#define RCconstPI   0.159154943092f // 0.5f / M_PI;
+#define MAIN_CUT_HZ 12.0f           // (default 12Hz, Range 1-50Hz)
+#define OLD_YAW     0               // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw.
+
+void pidHarakiri(void)
+{
+    float delta, RCfactor, rcCommandAxis, MainDptCut, gyroDataQuant;
+    float PTerm, ITerm, DTerm, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO, error, prop = 0.0f;
+    static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f };
+    uint8_t axis;
+    float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale;
+
+//    MainDptCut = RCconstPI / (float)cfg.maincuthz;                           // Initialize Cut off frequencies for mainpid D
+    MainDptCut = RCconstPI / MAIN_CUT_HZ;                                      // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now
+    FLOATcycleTime  = (float)constrain(cycleTime, 1, 100000);                  // 1us - 100ms
+    ACCDeltaTimeINS = FLOATcycleTime * 0.000001f;                              // ACCDeltaTimeINS is in seconds now
+    RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS);               // used for pt1 element
+
+    if (f.HORIZON_MODE) {
+        prop = (float)min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 450) / 450.0f;
+    }
+
+    for (axis = 0; axis < 2; axis++) {
+        int32_t tmp = (int32_t)((float)gyroData[axis] * 0.3125f);              // Multiwii masks out the last 2 bits, this has the same idea
+        gyroDataQuant = (float)tmp * 3.2f;                                     // but delivers more accuracy and also reduces jittery flight
+        rcCommandAxis = (float)rcCommand[axis];                                // Calculate common values for pid controllers
+        if (f.ANGLE_MODE || f.HORIZON_MODE) {
+            error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) mcfg.max_angle_inclination), +mcfg.max_angle_inclination) - angle[axis] + cfg.angleTrim[axis];
+            PTermACC = error * (float)cfg.P8[PIDLEVEL] * 0.008f;
+            float limitf = (float)cfg.D8[PIDLEVEL] * 5.0f;
+            PTermACC = constrain(PTermACC, -limitf, +limitf);
+            errorAngleIf[axis] = constrainf(errorAngleIf[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
+            ITermACC = errorAngleIf[axis] * (float)cfg.I8[PIDLEVEL] * 0.08f;
+        }
+
+        if (!f.ANGLE_MODE) {
+            if (abs((int16_t)gyroData[axis]) > 2560) {
+                errorGyroIf[axis] = 0.0f;
+            } else {
+                error = (rcCommandAxis * 320.0f / (float)cfg.P8[axis]) - gyroDataQuant;
+                errorGyroIf[axis] = constrainf(errorGyroIf[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
+            }
+
+            ITermGYRO = errorGyroIf[axis] * (float)cfg.I8[axis] * 0.01f;
+
+            if (f.HORIZON_MODE) {
+                PTerm = PTermACC + prop * (rcCommandAxis - PTermACC);
+                ITerm = ITermACC + prop * (ITermGYRO - ITermACC);
+            } else {
+                PTerm = rcCommandAxis;
+                ITerm = ITermGYRO;
+            }
+        } else {
+            PTerm = PTermACC;
+            ITerm = ITermACC;
+        }
+
+        PTerm -= gyroDataQuant * dynP8[axis] * 0.003f;
+        delta = (gyroDataQuant - lastGyro[axis]) / ACCDeltaTimeINS;
+
+        lastGyro[axis] = gyroDataQuant;
+        lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]);
+        DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f;
+
+        axisPID[axis] = lrintf(PTerm + ITerm - DTerm);                         // Round up result.
+    }
+
+    Mwii3msTimescale = (int32_t)FLOATcycleTime & (int32_t)~3;                  // Filter last 2 bit jitter
+    Mwii3msTimescale /= 3000.0f;
+
+    if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
+        PTerm = ((int32_t)cfg.P8[YAW] * (100 - (int32_t)cfg.yawRate * (int32_t)abs(rcCommand[YAW]) / 500)) / 100;
+        int32_t tmp = lrintf(gyroData[YAW] * 0.25f);
+        PTerm = rcCommand[YAW] - tmp * PTerm / 80;
+        if ((abs(tmp) > 640) || (abs(rcCommand[YAW]) > 100)) {
+            errorGyroI[YAW] = 0;
+        } else {
+            error = ((int32_t)rcCommand[YAW] * 80 / (int32_t)cfg.P8[YAW]) - tmp;
+            errorGyroI[YAW] = constrain(errorGyroI[YAW] + (int32_t)(error * Mwii3msTimescale), -16000, +16000); // WindUp
+            ITerm = (errorGyroI[YAW] / 125 * cfg.I8[YAW]) >> 6;
+        }
+    } else {
+        int32_t tmp = ((int32_t)rcCommand[YAW] * (((int32_t)cfg.yawRate << 1) + 40)) >> 5;
+        error = tmp - lrintf(gyroData[YAW] * 0.25f);                        // Less Gyrojitter works actually better
+        errorGyroI[YAW] = constrain(errorGyroI[YAW] + (int32_t)(error * (float)cfg.I8[YAW] * Mwii3msTimescale), -268435454, +268435454);
+
+        if (cfg.yawdeadband) {
+            if (rcCommand[YAW]) errorGyroI[YAW] = 0;
+        } else {
+            if (abs(tmp) > 50) errorGyroI[YAW] = 0;
+        }
+
+        ITerm = constrain(errorGyroI[YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX);
+        PTerm = ((int32_t)error * (int32_t)cfg.P8[YAW]) >> 6;
+
+        if (core.numMotors >= 4) { // Constrain YAW by D value if not servo driven in that case servolimits apply
+            int32_t limit = 300;
+            if (cfg.D8[YAW]) limit -= (int32_t)cfg.D8[YAW];
+            PTerm = constrain(PTerm, -limit, limit);
+        }
+    }
+    axisPID[YAW] = PTerm + ITerm;
+    axisPID[YAW] = lrintf(axisPID[YAW]);                                 // Round up result.
+}
+
+
 void setPIDController(int type)
 {
     switch (type) {
@@ -464,6 +572,9 @@ void setPIDController(int type)
         case 1:
             pid_controller = pidRewrite;
             break;
+        case 2:
+            pid_controller = pidHarakiri;
+            break;
     }
 }
 
@@ -571,6 +682,10 @@ void loop(void)
             errorGyroI[YAW] = 0;
             errorAngleI[ROLL] = 0;
             errorAngleI[PITCH] = 0;
+            errorGyroIf[ROLL] = 0.0f;
+            errorGyroIf[PITCH] = 0.0f;
+            errorAngleIf[ROLL] = 0.0f;
+            errorAngleIf[PITCH] = 0.0f;
             if (cfg.activate[BOXARM] > 0) { // Arming via ARM BOX
                 if (rcOptions[BOXARM] && f.OK_TO_ARM)
                     mwArm();

src/mw.h

@@ -402,6 +402,7 @@ typedef struct core_t {
     uint8_t mpu6050_scale;                  // es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES, need this to be dynamic. automatically set by mpu6050 driver.
     uint8_t numRCChannels;                  // number of rc channels as reported by current input driver
     bool useServo;                          // feature SERVO_TILT or wing/airplane mixers will enable this
+    uint8_t numMotors;                      // how many motors we have. used by mixer
     uint8_t numServos;                      // how many total hardware servos we have. used by mixer
 } core_t;
 

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;

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