ground/gcs/src/plugins/config/configautotunewidget.cpp

   1 #include "configautotunewidget.h"
   2
   3 #include "ui_autotune.h"
   4
   5 #include "relaytuningsettings.h"
   6 #include "relaytuning.h"
   7 #include "stabilizationsettings.h"
   8 #include "hwsettings.h"
   9
  10 #include <QDebug>
  11 #include <QStringList>
  12 #include <QWidget>
  13 #include <QTextEdit>
  14 #include <QVBoxLayout>
  15 #include <QPushButton>
  16 #include <QDesktopServices>
  17 #include <QUrl>
  18 #include <QList>
  19
  20 ConfigAutotuneWidget::ConfigAutotuneWidget(QWidget *parent) :
  21     ConfigTaskWidget(parent)
  22 {
  23     m_autotune = new Ui_AutotuneWidget();
  24     m_autotune->setupUi(this);
  25
  26     // Connect automatic signals
  27     autoLoadWidgets();
  28     disableMouseWheelEvents();
  29
  30     // Whenever any value changes compute new potential stabilization settings
  31     connect(m_autotune->rateTuning, SIGNAL(valueChanged(int)), this, SLOT(recomputeStabilization()));
  32     connect(m_autotune->attitudeTuning, SIGNAL(valueChanged(int)), this, SLOT(recomputeStabilization()));
  33
  34     addUAVObject("HwSettings");
  35     addWidget(m_autotune->enableAutoTune);
  36
  37     RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager());
  38     Q_ASSERT(relayTuning);
  39     if (relayTuning) {
  40         connect(relayTuning, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(recomputeStabilization()));
  41     }
  42
  43     // Connect the apply button for the stabilization settings
  44     connect(m_autotune->useComputedValues, SIGNAL(pressed()), this, SLOT(saveStabilization()));
  45 }
  46
  47 /**
  48  * Apply the stabilization settings computed
  49  */
  50 void ConfigAutotuneWidget::saveStabilization()
  51 {
  52     StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager());
  53
  54     Q_ASSERT(stabilizationSettings);
  55     if (!stabilizationSettings) {
  56         return;
  57     }
  58
  59     // Make sure to recompute in case the other stab settings changed since
  60     // the last time
  61     recomputeStabilization();
  62
  63     // Apply this data to the board
  64     stabilizationSettings->setData(stabSettings);
  65     stabilizationSettings->updated();
  66 }
  67
  68 /**
  69  * Called whenever the gain ratios or measured values
  70  * are changed
  71  */
  72 void ConfigAutotuneWidget::recomputeStabilization()
  73 {
  74     RelayTuningSettings *relayTuningSettings = RelayTuningSettings::GetInstance(getObjectManager());
  75
  76     Q_ASSERT(relayTuningSettings);
  77     if (!relayTuningSettings) {
  78         return;
  79     }
  80
  81     RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager());
  82     Q_ASSERT(relayTuning);
  83     if (!relayTuning) {
  84         return;
  85     }
  86
  87     StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager());
  88     Q_ASSERT(stabilizationSettings);
  89     if (!stabilizationSettings) {
  90         return;
  91     }
  92
  93     RelayTuning::DataFields relayTuningData = relayTuning->getData();
  94     RelayTuningSettings::DataFields tuningSettingsData = relayTuningSettings->getData();
  95     stabSettings = stabilizationSettings->getData();
  96
  97     // Need to divide these by 100 because that is what the .ui file does
  98     // to get the UAVO
  99     const double gain_ratio_r = m_autotune->rateTuning->value() / 100.0;
 100     const double zero_ratio_r = m_autotune->rateTuning->value() / 100.0;
 101     const double gain_ratio_p = m_autotune->attitudeTuning->value() / 100.0;
 102     const double zero_ratio_p = m_autotune->attitudeTuning->value() / 100.0;
 103
 104     // For now just run over roll and pitch
 105     for (int i = 0; i < 2; i++) {
 106         if (relayTuningData.Period[i] == 0 || relayTuningData.Gain[i] == 0) {
 107             continue;
 108         }
 109
 110         double wu  = 1000.0 * 2 * M_PI / relayTuningData.Period[i]; // ultimate freq = output osc freq (rad/s)
 111
 112         double wc  = wu * gain_ratio_r;      // target openloop crossover frequency (rad/s)
 113         double zc  = wc * zero_ratio_r;      // controller zero location (rad/s)
 114         double kpu = 4.0f / M_PI / relayTuningData.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity
 115         double kp  = kpu * gain_ratio_r;     // proportional gain
 116         double ki  = zc * kp;                // integral gain
 117
 118         // Now calculate gains for the next loop out knowing it is the integral of
 119         // the inner loop -- the plant is position/velocity = scale*1/s
 120         double wc2 = wc * gain_ratio_p; // crossover of the attitude loop
 121         double kp2 = wc2; // kp of attitude
 122         double ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude
 123
 124         switch (i) {
 125         case 0: // Roll
 126
 127             stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KP] = kp;
 128             stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KI] = ki;
 129             stabSettings.RollPI[StabilizationSettings::ROLLPI_KP] = kp2;
 130             stabSettings.RollPI[StabilizationSettings::ROLLPI_KI] = ki2;
 131             break;
 132         case 1: // Pitch
 133             stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KP] = kp;
 134             stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI] = ki;
 135             stabSettings.PitchPI[StabilizationSettings::PITCHPI_KP] = kp2;
 136             stabSettings.PitchPI[StabilizationSettings::PITCHPI_KI] = ki2;
 137             break;
 138         }
 139     }
 140
 141     // Display these computed settings
 142     m_autotune->rollRateKp->setText(QString().number(stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KP]));
 143     m_autotune->rollRateKi->setText(QString().number(stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KI]));
 144     m_autotune->rollAttitudeKp->setText(QString().number(stabSettings.RollPI[StabilizationSettings::ROLLPI_KP]));
 145     m_autotune->rollAttitudeKi->setText(QString().number(stabSettings.RollPI[StabilizationSettings::ROLLPI_KI]));
 146     m_autotune->pitchRateKp->setText(QString().number(stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KP]));
 147     m_autotune->pitchRateKi->setText(QString().number(stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI]));
 148     m_autotune->pitchAttitudeKp->setText(QString().number(stabSettings.PitchPI[StabilizationSettings::PITCHPI_KP]));
 149     m_autotune->pitchAttitudeKi->setText(QString().number(stabSettings.PitchPI[StabilizationSettings::PITCHPI_KI]));
 150 }
 151 void ConfigAutotuneWidget::refreshWidgetsValues(UAVObject *obj)
 152 {
 153     HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager());
 154
 155     if (obj == hwSettings) {
 156         bool dirtyBack = isDirty();
 157         HwSettings::DataFields hwSettingsData = hwSettings->getData();
 158         m_autotune->enableAutoTune->setChecked(
 159             hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_AUTOTUNE] == HwSettings::OPTIONALMODULES_ENABLED);
 160         setDirty(dirtyBack);
 161     }
 162     ConfigTaskWidget::refreshWidgetsValues(obj);
 163 }
 164 void ConfigAutotuneWidget::updateObjectsFromWidgets()
 165 {
 166     HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager());
 167     HwSettings::DataFields hwSettingsData = hwSettings->getData();
 168
 169     hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_AUTOTUNE] =
 170         m_autotune->enableAutoTune->isChecked() ? HwSettings::OPTIONALMODULES_ENABLED : HwSettings::OPTIONALMODULES_DISABLED;
 171     hwSettings->setData(hwSettingsData);
 172     ConfigTaskWidget::updateObjectsFromWidgets();
 173 }