From 97b4b786d145d77bc465a7c4c23fe40996927b55 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 21 Jan 2015 23:00:09 +0100 Subject: [PATCH] Remove yaw gyro scale for AlienWii 32 target, additional PID controllers BLACKBOX support --- src/main/flight/flight.c | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index c0a7d768bb..c7cdaee4a8 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -359,7 +359,11 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control //YAW rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; +#ifdef ALIENWII32 + error = rc - gyroData[FD_YAW]; +#else error = rc - (gyroData[FD_YAW] / 4); +#endif errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); if (abs(rc) > 50) errorGyroI[FD_YAW] = 0; @@ -375,6 +379,12 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); axisPID[FD_YAW] = PTerm + ITerm; + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif } static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, @@ -454,7 +464,11 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con } //YAW rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; +#ifdef ALIENWII32 + error = rc - gyroData[FD_YAW]; +#else error = rc - (gyroData[FD_YAW] / 4); +#endif errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); if (abs(rc) > 50) errorGyroI[FD_YAW] = 0; @@ -470,6 +484,12 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); axisPID[FD_YAW] = PTerm + ITerm; + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif } static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,