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

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