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