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

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       configtxpidswidget.cpp
   5  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
   6  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
   7  * @addtogroup GCSPlugins GCS Plugins
   8  * @{
   9  * @addtogroup ConfigPlugin Config Plugin
  10  * @{
  11  * @brief The Configuration Gadget used to configure TxPID module
  12  *****************************************************************************/
  13 /*
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License as published by
  16  * the Free Software Foundation; either version 3 of the License, or
  17  * (at your option) any later version.
  18  *
  19  * This program is distributed in the hope that it will be useful, but
  20  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  21  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  22  * for more details.
  23  *
  24  * You should have received a copy of the GNU General Public License along
  25  * with this program; if not, write to the Free Software Foundation, Inc.,
  26  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  27  */
  28
  29 #include "configtxpidwidget.h"
  30
  31 #include "ui_txpid.h"
  32
  33 #include <extensionsystem/pluginmanager.h>
  34 #include <coreplugin/generalsettings.h>
  35
  36 #include "txpidsettings.h"
  37 #include "hwsettings.h"
  38 #include "attitudesettings.h"
  39 #include "stabilizationsettings.h"
  40 #include "stabilizationsettingsbank1.h"
  41 #include "stabilizationsettingsbank2.h"
  42 #include "stabilizationsettingsbank3.h"
  43
  44 ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent)
  45 {
  46     m_txpid = new Ui_TxPIDWidget();
  47     m_txpid->setupUi(this);
  48
  49     setWikiURL("TxPID");
  50     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
  51     Core::Internal::GeneralSettings *settings = pm->getObject<Core::Internal::GeneralSettings>();
  52     if (!settings->useExpertMode()) {
  53         m_txpid->Apply->setVisible(false);
  54     }
  55     autoLoadWidgets();
  56     addApplySaveButtons(m_txpid->Apply, m_txpid->Save);
  57
  58     // Cannot use addUAVObjectToWidgetRelation() for OptionaModules enum because
  59     // QCheckBox returns bool (0 or -1) and this value is then set to enum instead
  60     // or enum options
  61     connect(HwSettings::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(refreshValues()));
  62     connect(m_txpid->Apply, SIGNAL(clicked()), this, SLOT(applySettings()));
  63     connect(m_txpid->Save, SIGNAL(clicked()), this, SLOT(saveSettings()));
  64
  65     connect(m_txpid->PID1, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSpinBoxProperties(int)));
  66     connect(m_txpid->PID2, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSpinBoxProperties(int)));
  67     connect(m_txpid->PID3, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSpinBoxProperties(int)));
  68
  69     addWidgetBinding("TxPIDSettings", "BankNumber", m_txpid->pidBank, 0, 1, true);
  70
  71     addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input1, TxPIDSettings::INPUTS_INSTANCE1);
  72     addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input2, TxPIDSettings::INPUTS_INSTANCE2);
  73     addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input3, TxPIDSettings::INPUTS_INSTANCE3);
  74
  75     // It's important that the PIDx values are populated before the MinPIDx and MaxPIDx,
  76     // otherwise the MinPIDx and MaxPIDx will be capped by the old spin box limits. The correct limits
  77     // are set when updateSpinBoxProperties is called when the PIDx->currentTextChanged signal is sent.
  78     // The binding order is reversed because the values are populated in reverse.
  79
  80     addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID1, TxPIDSettings::MINPID_INSTANCE1);
  81     addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID2, TxPIDSettings::MINPID_INSTANCE2);
  82     addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID3, TxPIDSettings::MINPID_INSTANCE3);
  83
  84     addWidgetBinding("TxPIDSettings", "MaxPID", m_txpid->MaxPID1, TxPIDSettings::MAXPID_INSTANCE1);
  85     addWidgetBinding("TxPIDSettings", "MaxPID", m_txpid->MaxPID2, TxPIDSettings::MAXPID_INSTANCE2);
  86     addWidgetBinding("TxPIDSettings", "MaxPID", m_txpid->MaxPID3, TxPIDSettings::MAXPID_INSTANCE3);
  87
  88     addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID1, TxPIDSettings::PIDS_INSTANCE1);
  89     addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID2, TxPIDSettings::PIDS_INSTANCE2);
  90     addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID3, TxPIDSettings::PIDS_INSTANCE3);
  91
  92     addWidgetBinding("TxPIDSettings", "ThrottleRange", m_txpid->ThrottleMin, TxPIDSettings::THROTTLERANGE_MIN);
  93     addWidgetBinding("TxPIDSettings", "ThrottleRange", m_txpid->ThrottleMax, TxPIDSettings::THROTTLERANGE_MAX);
  94
  95     addWidgetBinding("TxPIDSettings", "UpdateMode", m_txpid->UpdateMode);
  96
  97     connect(this, SIGNAL(widgetContentsChanged(QWidget *)), this, SLOT(processLinkedWidgets(QWidget *)));
  98
  99     addWidget(m_txpid->TxPIDEnable);
 100     addWidget(m_txpid->enableAutoCalcYaw);
 101     enableControls(false);
 102     populateWidgets();
 103     refreshWidgetsValues();
 104
 105     disableMouseWheelEvents();
 106 }
 107
 108 ConfigTxPIDWidget::~ConfigTxPIDWidget()
 109 {
 110     // Do nothing
 111 }
 112
 113 static bool isResponsivenessOption(int pidOption)
 114 {
 115     switch (pidOption) {
 116     case TxPIDSettings::PIDS_ROLLRATERESP:
 117     case TxPIDSettings::PIDS_PITCHRATERESP:
 118     case TxPIDSettings::PIDS_ROLLPITCHRATERESP:
 119     case TxPIDSettings::PIDS_YAWRATERESP:
 120     case TxPIDSettings::PIDS_ROLLATTITUDERESP:
 121     case TxPIDSettings::PIDS_PITCHATTITUDERESP:
 122     case TxPIDSettings::PIDS_ROLLPITCHATTITUDERESP:
 123     case TxPIDSettings::PIDS_YAWATTITUDERESP:
 124         return true;
 125
 126     default:
 127         return false;
 128     }
 129 }
 130
 131 static bool isAttitudeOption(int pidOption)
 132 {
 133     switch (pidOption) {
 134     case TxPIDSettings::PIDS_ROLLATTITUDEKP:
 135     case TxPIDSettings::PIDS_PITCHATTITUDEKP:
 136     case TxPIDSettings::PIDS_ROLLPITCHATTITUDEKP:
 137     case TxPIDSettings::PIDS_YAWATTITUDEKP:
 138     case TxPIDSettings::PIDS_ROLLATTITUDEKI:
 139     case TxPIDSettings::PIDS_PITCHATTITUDEKI:
 140     case TxPIDSettings::PIDS_ROLLPITCHATTITUDEKI:
 141     case TxPIDSettings::PIDS_YAWATTITUDEKI:
 142     case TxPIDSettings::PIDS_ROLLATTITUDEILIMIT:
 143     case TxPIDSettings::PIDS_PITCHATTITUDEILIMIT:
 144     case TxPIDSettings::PIDS_ROLLPITCHATTITUDEILIMIT:
 145     case TxPIDSettings::PIDS_YAWATTITUDEILIMIT:
 146     case TxPIDSettings::PIDS_ROLLATTITUDERESP:
 147     case TxPIDSettings::PIDS_PITCHATTITUDERESP:
 148     case TxPIDSettings::PIDS_ROLLPITCHATTITUDERESP:
 149     case TxPIDSettings::PIDS_YAWATTITUDERESP:
 150         return true;
 151
 152     default:
 153         return false;
 154     }
 155 }
 156
 157 static bool isExpoOption(int pidOption)
 158 {
 159     switch (pidOption) {
 160     case TxPIDSettings::PIDS_ROLLEXPO:
 161     case TxPIDSettings::PIDS_PITCHEXPO:
 162     case TxPIDSettings::PIDS_ROLLPITCHEXPO:
 163     case TxPIDSettings::PIDS_YAWEXPO:
 164         return true;
 165
 166     default:
 167         return false;
 168     }
 169 }
 170
 171 static bool isFullPIDOption(int pidOption)
 172 {
 173     switch (pidOption) {
 174     case TxPIDSettings::PIDS_EASYTUNERATEPITCH:
 175     case TxPIDSettings::PIDS_EASYTUNERATEROLL:
 176         return true;
 177
 178     default:
 179         return false;
 180     }
 181 }
 182
 183 static bool isAcroPlusFactorOption(int pidOption)
 184 {
 185     switch (pidOption) {
 186     case TxPIDSettings::PIDS_ACROPITCHFACTOR:
 187     case TxPIDSettings::PIDS_ACROROLLFACTOR:
 188     case TxPIDSettings::PIDS_ACROROLLPITCHFACTOR:
 189         return true;
 190
 191     default:
 192         return false;
 193     }
 194 }
 195
 196 template <class StabilizationSettingsBankX>
 197 static float defaultValueForPidOption(const StabilizationSettingsBankX *bank, int pidOption)
 198 {
 199     switch (pidOption) {
 200     case TxPIDSettings::PIDS_DISABLED:
 201         return 0.0f;
 202
 203     case TxPIDSettings::PIDS_ROLLRATEKP:
 204     case TxPIDSettings::PIDS_EASYTUNERATEROLL:
 205         return bank->getRollRatePID_Kp();
 206
 207     case TxPIDSettings::PIDS_PITCHRATEKP:
 208     case TxPIDSettings::PIDS_EASYTUNERATEPITCH:
 209         return bank->getPitchRatePID_Kp();
 210
 211     case TxPIDSettings::PIDS_ROLLPITCHRATEKP:
 212         return bank->getRollRatePID_Kp();
 213
 214     case TxPIDSettings::PIDS_YAWRATEKP:
 215         return bank->getYawRatePID_Kp();
 216
 217     case TxPIDSettings::PIDS_ROLLRATEKI:
 218         return bank->getRollRatePID_Ki();
 219
 220     case TxPIDSettings::PIDS_PITCHRATEKI:
 221         return bank->getPitchRatePID_Ki();
 222
 223     case TxPIDSettings::PIDS_ROLLPITCHRATEKI:
 224         return bank->getRollRatePID_Ki();
 225
 226     case TxPIDSettings::PIDS_YAWRATEKI:
 227         return bank->getYawRatePID_Ki();
 228
 229     case TxPIDSettings::PIDS_ROLLRATEKD:
 230         return bank->getRollRatePID_Kd();
 231
 232     case TxPIDSettings::PIDS_PITCHRATEKD:
 233         return bank->getPitchRatePID_Kd();
 234
 235     case TxPIDSettings::PIDS_ROLLPITCHRATEKD:
 236         return bank->getRollRatePID_Kd();
 237
 238     case TxPIDSettings::PIDS_YAWRATEKD:
 239         return bank->getYawRatePID_Kd();
 240
 241     case TxPIDSettings::PIDS_ROLLRATEILIMIT:
 242         return bank->getRollRatePID_ILimit();
 243
 244     case TxPIDSettings::PIDS_PITCHRATEILIMIT:
 245         return bank->getPitchRatePID_ILimit();
 246
 247     case TxPIDSettings::PIDS_ROLLPITCHRATEILIMIT:
 248         return bank->getRollRatePID_ILimit();
 249
 250     case TxPIDSettings::PIDS_YAWRATEILIMIT:
 251         return bank->getYawRatePID_ILimit();
 252
 253     case TxPIDSettings::PIDS_ROLLRATERESP:
 254         return bank->getManualRate_Roll();
 255
 256     case TxPIDSettings::PIDS_PITCHRATERESP:
 257         return bank->getManualRate_Pitch();
 258
 259     case TxPIDSettings::PIDS_ROLLPITCHRATERESP:
 260         return bank->getManualRate_Roll();
 261
 262     case TxPIDSettings::PIDS_YAWRATERESP:
 263         return bank->getManualRate_Yaw();
 264
 265     case TxPIDSettings::PIDS_ROLLATTITUDEKP:
 266         return bank->getRollPI_Kp();
 267
 268     case TxPIDSettings::PIDS_PITCHATTITUDEKP:
 269         return bank->getPitchPI_Kp();
 270
 271     case TxPIDSettings::PIDS_ROLLPITCHATTITUDEKP:
 272         return bank->getRollPI_Kp();
 273
 274     case TxPIDSettings::PIDS_YAWATTITUDEKP:
 275         return bank->getYawPI_Kp();
 276
 277     case TxPIDSettings::PIDS_ROLLATTITUDEKI:
 278         return bank->getRollPI_Ki();
 279
 280     case TxPIDSettings::PIDS_PITCHATTITUDEKI:
 281         return bank->getPitchPI_Ki();
 282
 283     case TxPIDSettings::PIDS_ROLLPITCHATTITUDEKI:
 284         return bank->getRollPI_Ki();
 285
 286     case TxPIDSettings::PIDS_YAWATTITUDEKI:
 287         return bank->getYawPI_Ki();
 288
 289     case TxPIDSettings::PIDS_ROLLATTITUDEILIMIT:
 290         return bank->getRollPI_ILimit();
 291
 292     case TxPIDSettings::PIDS_PITCHATTITUDEILIMIT:
 293         return bank->getPitchPI_ILimit();
 294
 295     case TxPIDSettings::PIDS_ROLLPITCHATTITUDEILIMIT:
 296         return bank->getRollPI_ILimit();
 297
 298     case TxPIDSettings::PIDS_YAWATTITUDEILIMIT:
 299         return bank->getYawPI_ILimit();
 300
 301     case TxPIDSettings::PIDS_ROLLATTITUDERESP:
 302         return (float)bank->getRollMax();
 303
 304     case TxPIDSettings::PIDS_PITCHATTITUDERESP:
 305         return (float)bank->getPitchMax();
 306
 307     case TxPIDSettings::PIDS_ROLLPITCHATTITUDERESP:
 308         return (float)bank->getRollMax();
 309
 310     case TxPIDSettings::PIDS_YAWATTITUDERESP:
 311         return bank->getYawMax();
 312
 313     case TxPIDSettings::PIDS_ROLLEXPO:
 314         return bank->getStickExpo_Roll();
 315
 316     case TxPIDSettings::PIDS_PITCHEXPO:
 317         return bank->getStickExpo_Pitch();
 318
 319     case TxPIDSettings::PIDS_ROLLPITCHEXPO:
 320         return bank->getStickExpo_Roll();
 321
 322     case TxPIDSettings::PIDS_YAWEXPO:
 323         return bank->getStickExpo_Yaw();
 324
 325     case TxPIDSettings::PIDS_ACROROLLFACTOR:
 326     case TxPIDSettings::PIDS_ACROROLLPITCHFACTOR:
 327         return bank->getAcroInsanityFactor_Roll();
 328
 329     case TxPIDSettings::PIDS_ACROPITCHFACTOR:
 330         return bank->getAcroInsanityFactor_Pitch();
 331
 332     case -1: // The PID Option field was uninitialized.
 333         return 0.0f;
 334
 335     default:
 336         Q_ASSERT_X(false, "getDefaultValueForOption", "Incorrect PID option");
 337         return 0.0f;
 338     }
 339 }
 340
 341 float ConfigTxPIDWidget::getDefaultValueForPidOption(int pidOption)
 342 {
 343     if (pidOption == TxPIDSettings::PIDS_GYROTAU) {
 344         StabilizationSettings *stab = qobject_cast<StabilizationSettings *>(getObject(QString("StabilizationSettings")));
 345         return stab->getGyroTau();
 346     } else if (pidOption == TxPIDSettings::PIDS_ACCELTAU) {
 347         AttitudeSettings *att = qobject_cast<AttitudeSettings *>(getObject(QString("AttitudeSettings")));
 348         return att->getAccelTau();
 349     } else if (pidOption == TxPIDSettings::PIDS_ACCELKP) {
 350         AttitudeSettings *att = qobject_cast<AttitudeSettings *>(getObject(QString("AttitudeSettings")));
 351         return att->getAccelKp();
 352     } else if (pidOption == TxPIDSettings::PIDS_ACCELKI) {
 353         AttitudeSettings *att = qobject_cast<AttitudeSettings *>(getObject(QString("AttitudeSettings")));
 354         return att->getAccelKi();
 355     }
 356
 357     int pidBankIndex = m_txpid->pidBank->currentIndex();
 358
 359     if (pidBankIndex == -1) {
 360         // The pidBank field was uninitilized.
 361         return 0.0f;
 362     }
 363
 364     int bankNumber = pidBankIndex + 1;
 365
 366     if (bankNumber == 1) {
 367         StabilizationSettingsBank1 *bank = qobject_cast<StabilizationSettingsBank1 *>(getObject(QString("StabilizationSettingsBank1")));
 368         return defaultValueForPidOption(bank, pidOption);
 369     } else if (bankNumber == 2) {
 370         StabilizationSettingsBank2 *bank = qobject_cast<StabilizationSettingsBank2 *>(getObject(QString("StabilizationSettingsBank2")));
 371         return defaultValueForPidOption(bank, pidOption);
 372     } else if (bankNumber == 3) {
 373         StabilizationSettingsBank3 *bank = qobject_cast<StabilizationSettingsBank3 *>(getObject(QString("StabilizationSettingsBank3")));
 374         return defaultValueForPidOption(bank, pidOption);
 375     } else {
 376         Q_ASSERT_X(false, "getDefaultValueForPidOption", "Incorrect bank number");
 377         return 0.0f;
 378     }
 379 }
 380
 381 void ConfigTxPIDWidget::updateSpinBoxProperties(int selectedPidOption)
 382 {
 383     QObject *PIDx = sender();
 384
 385     QDoubleSpinBox *minPID;
 386     QDoubleSpinBox *maxPID;
 387
 388     if (PIDx == m_txpid->PID1) {
 389         minPID = m_txpid->MinPID1;
 390         maxPID = m_txpid->MaxPID1;
 391     } else if (PIDx == m_txpid->PID2) {
 392         minPID = m_txpid->MinPID2;
 393         maxPID = m_txpid->MaxPID2;
 394     } else if (PIDx == m_txpid->PID3) {
 395         minPID = m_txpid->MinPID3;
 396         maxPID = m_txpid->MaxPID3;
 397     } else {
 398         Q_ASSERT_X(false, "updateSpinBoxProperties", "Incorrect sender object");
 399         return;
 400     }
 401
 402     // The ranges need to be setup before the values can be set,
 403     // otherwise the value might be incorrectly capped.
 404
 405     if (isResponsivenessOption(selectedPidOption)) {
 406         if (isAttitudeOption(selectedPidOption)) {
 407             // Limit to 180 degrees.
 408             minPID->setRange(0, 180);
 409             maxPID->setRange(0, 180);
 410         } else {
 411             minPID->setRange(0, 999);
 412             maxPID->setRange(0, 999);
 413         }
 414         minPID->setSingleStep(1);
 415         maxPID->setSingleStep(1);
 416         minPID->setDecimals(0);
 417         maxPID->setDecimals(0);
 418     } else if (isExpoOption(selectedPidOption)) {
 419         minPID->setRange(-100, 100);
 420         maxPID->setRange(-100, 100);
 421         minPID->setSingleStep(1);
 422         maxPID->setSingleStep(1);
 423         minPID->setDecimals(0);
 424         maxPID->setDecimals(0);
 425     } else if (isAcroPlusFactorOption(selectedPidOption)) {
 426         minPID->setRange(0, 100);
 427         maxPID->setRange(0, 100);
 428         minPID->setSingleStep(1);
 429         maxPID->setSingleStep(1);
 430         minPID->setDecimals(0);
 431         maxPID->setDecimals(0);
 432     } else {
 433         minPID->setRange(0, 99.99);
 434         maxPID->setRange(0, 99.99);
 435         minPID->setSingleStep(0.000100);
 436         maxPID->setSingleStep(0.000100);
 437         minPID->setDecimals(6);
 438         maxPID->setDecimals(6);
 439     }
 440
 441     float value = getDefaultValueForPidOption(selectedPidOption);
 442
 443     minPID->setValue(value);
 444     maxPID->setValue(value);
 445 }
 446
 447 void ConfigTxPIDWidget::refreshValues()
 448 {
 449     HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager());
 450     HwSettings::DataFields hwSettingsData = hwSettings->getData();
 451
 452     m_txpid->TxPIDEnable->setChecked(
 453         hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_TXPID] == HwSettings::OPTIONALMODULES_ENABLED);
 454 }
 455
 456 void ConfigTxPIDWidget::applySettings()
 457 {
 458     HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager());
 459     HwSettings::DataFields hwSettingsData = hwSettings->getData();
 460
 461     hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_TXPID] =
 462         m_txpid->TxPIDEnable->isChecked() ? HwSettings::OPTIONALMODULES_ENABLED : HwSettings::OPTIONALMODULES_DISABLED;
 463     hwSettings->setData(hwSettingsData);
 464 }
 465
 466 void ConfigTxPIDWidget::saveSettings()
 467 {
 468     applySettings();
 469     UAVObject *obj = HwSettings::GetInstance(getObjectManager());
 470     saveObjectToSD(obj);
 471 }
 472
 473 void ConfigTxPIDWidget::processLinkedWidgets(QWidget *widget)
 474 {
 475     Q_UNUSED(widget);
 476     bool fullPidEnabled =
 477         isFullPIDOption(m_txpid->PID1->currentIndex()) ||
 478         isFullPIDOption(m_txpid->PID2->currentIndex()) ||
 479         isFullPIDOption(m_txpid->PID3->currentIndex());
 480     bool calcYawEnabled = fullPidEnabled && m_txpid->enableAutoCalcYaw->isChecked();
 481
 482     m_txpid->fullPID_Y_P_FactorSlider->setEnabled(calcYawEnabled);
 483     m_txpid->fullPID_Y_P_FactorSpinBox->setEnabled(calcYawEnabled);
 484     m_txpid->fullPID_Y_I_FactorSpinBox->setEnabled(calcYawEnabled);
 485     m_txpid->fullPID_Y_D_FactorSpinBox->setEnabled(calcYawEnabled);
 486     m_txpid->enableAutoCalcYaw->setEnabled(fullPidEnabled);
 487     m_txpid->fullPID_RP_I_FactorSlider->setEnabled(fullPidEnabled);
 488     m_txpid->fullPID_RP_I_FactorSpinBox->setEnabled(fullPidEnabled);
 489     m_txpid->fullPID_RP_D_FactorSpinBox->setEnabled(fullPidEnabled);
 490     m_txpid->groupBox_FullPids->setEnabled(fullPidEnabled);
 491 }