src/main/flight/gtune.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21 #include <math.h>
  22
  23 #include "platform.h"
  24
  25 #ifdef GTUNE
  26
  27 #include "common/axis.h"
  28 #include "common/maths.h"
  29
  30 #include "drivers/system.h"
  31 #include "drivers/sensor.h"
  32 #include "drivers/accgyro.h"
  33
  34 #include "sensors/sensors.h"
  35 #include "sensors/gyro.h"
  36 #include "sensors/acceleration.h"
  37
  38 #include "flight/pid.h"
  39 #include "flight/imu.h"
  40
  41 #include "config/config.h"
  42 #include "blackbox/blackbox.h"
  43
  44 #include "io/rc_controls.h"
  45
  46 #include "config/runtime_config.h"
  47
  48 extern uint16_t cycleTime;
  49 extern uint8_t motorCount;
  50
  51 /*
  52  ****************************************************************************
  53  ***                    G_Tune                                            ***
  54  ****************************************************************************
  55         G_Tune Mode
  56         This is the multiwii implementation of ZERO-PID Algorithm
  57         http://technicaladventure.blogspot.com/2014/06/zero-pids-tuner-for-multirotors.html
  58         The algorithm has been originally developed by Mohammad Hefny (mohammad.hefny@gmail.com)
  59
  60         You may use/modify this algorithm on your own risk, kindly refer to above link in any future distribution.
  61  */
  62
  63 /*
  64    version 1.0.0: MIN & Maxis & Tuned Band
  65    version 1.0.1:
  66                 a. error is gyro reading not rc - gyro.
  67                 b. OldError = Error no averaging.
  68                 c. No Min Maxis BOUNDRY
  69     version 1.0.2:
  70                 a. no boundaries
  71                 b. I - Factor tune.
  72                 c. time_skip
  73
  74    Crashpilot: Reduced to just P tuning in a predefined range - so it is not "zero pid" anymore.
  75    Tuning is limited to just work when stick is centered besides that YAW is tuned in non Acro as well.
  76    See also:
  77    http://diydrones.com/profiles/blogs/zero-pid-tunes-for-multirotors-part-2
  78    http://www.multiwii.com/forum/viewtopic.php?f=8&t=5190
  79    Gyrosetting 2000DPS
  80    GyroScale = (1 / 16,4 ) * RADX(see board.h) = 0,001064225154 digit per rad/s
  81
  82     pidProfile->gtune_lolimP[ROLL]   = 10;  [0..200] Lower limit of ROLL P during G tune.
  83     pidProfile->gtune_lolimP[PITCH]  = 10;  [0..200] Lower limit of PITCH P during G tune.
  84     pidProfile->gtune_lolimP[YAW]    = 10;  [0..200] Lower limit of YAW P during G tune.
  85     pidProfile->gtune_hilimP[ROLL]   = 100; [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axisis.
  86     pidProfile->gtune_hilimP[PITCH]  = 100; [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axisis.
  87     pidProfile->gtune_hilimP[YAW]    = 100; [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axisis.
  88     pidProfile->gtune_pwr            = 0;   [0..10] Strength of adjustment
  89     pidProfile->gtune_settle_time    = 450; [200..1000] Settle time in ms
  90     pidProfile->gtune_average_cycles = 16;  [8..128] Number of looptime cycles used for gyro average calculation
  91 */
  92
  93 static pidProfile_t *pidProfile;
  94 static int16_t delay_cycles;
  95 static int16_t time_skip[3];
  96 static int16_t OldError[3], result_P64[3];
  97 static int32_t AvgGyro[3];
  98 static bool floatPID;
  99
 100 void updateDelayCycles(void)
 101 {
 102     delay_cycles = -(((int32_t)pidProfile->gtune_settle_time * 1000) / cycleTime);
 103 }
 104
 105 void init_Gtune(pidProfile_t *pidProfileToTune)
 106 {
 107     uint8_t i;
 108
 109     pidProfile = pidProfileToTune;
 110         if (pidProfile->pidController == 2) {
 111             floatPID = true;                                                        // LuxFloat is using float values for PID settings
 112         } else {
 113             floatPID = false;
 114         }
 115         updateDelayCycles();
 116         for (i = 0; i < 3; i++) {
 117         if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || (motorCount < 4 && i == FD_YAW)) { // User config error disable axisis for tuning
 118             pidProfile->gtune_hilimP[i] = 0;                                    // Disable YAW tuning for everything below a quadcopter
 119         }
 120         if (floatPID) {
 121             if((pidProfile->P_f[i] * 10.0f) < pidProfile->gtune_lolimP[i]) {
 122                 pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10.0f);
 123             }
 124             result_P64[i] = (int16_t)pidProfile->P_f[i] << 6;                   // 6 bit extra resolution for P.
 125         } else {
 126             if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) {
 127                 pidProfile->P8[i] = pidProfile->gtune_lolimP[i];
 128             }
 129             result_P64[i] = (int16_t)pidProfile->P8[i] << 6;                    // 6 bit extra resolution for P.
 130         }
 131         OldError[i] = 0;
 132         time_skip[i] = delay_cycles;
 133     }
 134 }
 135
 136 void calculate_Gtune(uint8_t axis)
 137 {
 138     int16_t error, diff_G, threshP;
 139
 140     if(rcCommand[axis] || (axis != FD_YAW && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)))) {  // Block tuning on stick input. Always allow G-Tune on YAW, Roll & Pitch only in acromode
 141         OldError[axis] = 0;
 142         time_skip[axis] = delay_cycles;                                         // Some settle time after stick center. default 450ms
 143     } else {
 144         if (!time_skip[axis]) AvgGyro[axis] = 0;
 145         time_skip[axis]++;
 146         if (time_skip[axis] > 0) {
 147             if (axis == FD_YAW) {
 148                 AvgGyro[axis] += 32 * ((int16_t)gyroADC[axis] / 32);           // Chop some jitter and average
 149             } else {
 150                 AvgGyro[axis] += 128 * ((int16_t)gyroADC[axis] / 128);         // Chop some jitter and average
 151             }
 152         }
 153         if (time_skip[axis] == pidProfile->gtune_average_cycles) {              // Looptime cycles for gyro average calculation. default 16.
 154             AvgGyro[axis] /= time_skip[axis];                                   // AvgGyro[axis] has now very clean gyrodata
 155             time_skip[axis] = 0;
 156             if (axis == FD_YAW) {
 157                 threshP = 20;
 158                 error = -AvgGyro[axis];
 159             } else {
 160                 threshP = 10;
 161                 error = AvgGyro[axis];
 162             }
 163             if (pidProfile->gtune_hilimP[axis] && error && OldError[axis] && error != OldError[axis]) {  // Don't run when not needed or pointless to do so
 164                 diff_G = ABS(error) - ABS(OldError[axis]);
 165                 if ((error > 0 && OldError[axis] > 0) || (error < 0 && OldError[axis] < 0)) {
 166                     if (diff_G > threshP) {
 167                         if (axis == FD_YAW) {
 168                             result_P64[axis] += 256 + pidProfile->gtune_pwr;    // YAW ends up at low limit on float PID, give it some more to work with.
 169                         } else {
 170                             result_P64[axis] += 64 + pidProfile->gtune_pwr;     // Shift balance a little on the plus side.
 171                         }
 172                     } else {
 173                         if (diff_G < -threshP) {
 174                             if (axis == FD_YAW) {
 175                                 result_P64[axis] -= 64 + pidProfile->gtune_pwr;
 176                             } else {
 177                                 result_P64[axis] -= 32;
 178                             }
 179                         }
 180                     }
 181                 } else {
 182                     if (ABS(diff_G) > threshP && axis != FD_YAW) {
 183                         result_P64[axis] -= 32;                                 // Don't use antiwobble for YAW
 184                     }
 185                 }
 186                 int16_t newP = constrain((result_P64[axis] >> 6), (int16_t)pidProfile->gtune_lolimP[axis], (int16_t)pidProfile->gtune_hilimP[axis]);
 187
 188 /*#ifdef BLACKBOX
 189                 if (feature(FEATURE_BLACKBOX)) {
 190                     flightLogEvent_gtuneCycleResult_t eventData;
 191
 192                     eventData.gtuneAxis = axis;
 193                     eventData.gtuneGyroAVG = AvgGyro[axis];
 194                     eventData.gtuneNewP = newP;                                 // for float PID the logged P value is still mutiplyed by 10
 195                     blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData);
 196                 }
 197 #endif*/
 198
 199                 if (floatPID) {
 200                     pidProfile->P_f[axis] = (float)newP / 10.0f;                // new P value for float PID
 201                 } else {
 202                     pidProfile->P8[axis] = newP;                                // new P value
 203                 }
 204             }
 205             OldError[axis] = error;
 206         }
 207     }
 208 }
 209
 210 #endif
 211