flight/modules/Stabilization/relay_tuning.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup StabilizationModule Stabilization Module
   6  * @brief Relay tuning controller
   7  * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
   8  * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
   9  * @{
  10  *
  11  * @file       stabilization.c
  12  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  13  * @brief      Attitude stabilization module.
  14  *
  15  * @see        The GNU Public License (GPL) Version 3
  16  *
  17  *****************************************************************************/
  18 /*
  19  * This program is free software; you can redistribute it and/or modify
  20  * it under the terms of the GNU General Public License as published by
  21  * the Free Software Foundation; either version 3 of the License, or
  22  * (at your option) any later version.
  23  *
  24  * This program is distributed in the hope that it will be useful, but
  25  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  26  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  27  * for more details.
  28  *
  29  * You should have received a copy of the GNU General Public License along
  30  * with this program; if not, write to the Free Software Foundation, Inc.,
  31  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  32  */
  33
  34 #include "openpilot.h"
  35 #include "stabilization.h"
  36 #include "relaytuning.h"
  37 #include "relaytuningsettings.h"
  38 #include "sin_lookup.h"
  39
  40 /**
  41  * Apply a step function for the stabilization controller and monitor the
  42  * result
  43  *
  44  * Used to  Replace the rate PID with a relay to measure the critical properties of this axis
  45  * i.e. period and gain
  46  */
  47 int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
  48 {
  49     RelayTuningData relay;
  50
  51     RelayTuningGet(&relay);
  52
  53     static portTickType lastHighTime;
  54     static portTickType lastLowTime;
  55
  56     static float accum_sin, accum_cos;
  57     static uint32_t accumulated  = 0;
  58
  59     const uint16_t DEGLITCH_TIME = 20; // ms
  60     const float AMPLITUDE_ALPHA  = 0.95f;
  61     const float PERIOD_ALPHA     = 0.95f;
  62
  63     portTickType thisTime = xTaskGetTickCount();
  64
  65     static bool rateRelayRunning[3];
  66
  67     // This indicates the current estimate of the smoothed error.  So when it is high
  68     // we are waiting for it to go low.
  69     static bool high = false;
  70
  71     // On first run initialize estimates to something reasonable
  72     if (reinit) {
  73         rateRelayRunning[axis] = false;
  74         RelayTuningPeriodToArray(relay.Period)[axis] = 200;
  75         RelayTuningGainToArray(relay.Gain)[axis]     = 0;
  76
  77         accum_sin   = 0;
  78         accum_cos   = 0;
  79         accumulated = 0;
  80
  81         // These should get reinitialized anyway
  82         high = true;
  83         lastHighTime = thisTime;
  84         lastLowTime  = thisTime;
  85         RelayTuningSet(&relay);
  86     }
  87
  88
  89     RelayTuningSettingsData relaySettings;
  90     RelayTuningSettingsGet(&relaySettings);
  91
  92     // Compute output, simple threshold on error
  93     *output = high ? relaySettings.Amplitude : -relaySettings.Amplitude;
  94
  95     /**** The code below here is to estimate the properties of the oscillation ****/
  96
  97     // Make sure the period can't go below limit
  98     if (RelayTuningPeriodToArray(relay.Period)[axis] < DEGLITCH_TIME) {
  99         RelayTuningPeriodToArray(relay.Period)[axis] = DEGLITCH_TIME;
 100     }
 101
 102     // Project the error onto a sine and cosine of the same frequency
 103     // to accumulate the average amplitude
 104     int32_t dT  = thisTime - lastHighTime;
 105     float phase = ((float)360 * (float)dT) / RelayTuningPeriodToArray(relay.Period)[axis];
 106     if (phase >= 360) {
 107         phase = 0;
 108     }
 109     accum_sin += sin_lookup_deg(phase) * error;
 110     accum_cos += cos_lookup_deg(phase) * error;
 111     accumulated++;
 112
 113     // Make sure we've had enough time since last transition then check for a change in the output
 114     bool time_hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME;
 115
 116     if (!high && time_hysteresis && error > relaySettings.HysteresisThresh) {
 117         /* POSITIVE CROSSING DETECTED */
 118
 119         float this_amplitude = 2 * sqrtf(accum_sin * accum_sin + accum_cos * accum_cos) / accumulated;
 120         float this_gain = this_amplitude / relaySettings.Amplitude;
 121
 122         accumulated = 0;
 123         accum_sin   = 0;
 124         accum_cos   = 0;
 125
 126         if (rateRelayRunning[axis] == false) {
 127             rateRelayRunning[axis] = true;
 128             RelayTuningPeriodToArray(relay.Period)[axis] = 200;
 129             RelayTuningGainToArray(relay.Gain)[axis] = 0;
 130         } else {
 131             // Low pass filter each amplitude and period
 132             RelayTuningGainToArray(relay.Gain)[axis]     =
 133                 RelayTuningGainToArray(relay.Gain)[axis] *
 134                 AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA);
 135             RelayTuningPeriodToArray(relay.Period)[axis] =
 136                 RelayTuningPeriodToArray(relay.Period)[axis] *
 137                 PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA);
 138         }
 139         lastHighTime = thisTime;
 140         high = true;
 141         RelayTuningSet(&relay);
 142     } else if (high && time_hysteresis && error < -relaySettings.HysteresisThresh) {
 143         /* FALLING CROSSING DETECTED */
 144
 145         lastLowTime = thisTime;
 146         high = false;
 147     }
 148
 149     return 0;
 150 }