void ConfigStabilizationWidget::resetToDefaults()
{
StabilizationSettings stabDefaults;
StabilizationSettings::DataFields defaults = stabDefaults.getData();
bool dirty=isDirty();
refreshUIValues(defaults);
setDirty(dirty);
}
void VehicleConfigurationHelper::applyStabilizationConfiguration()
{
StabilizationSettings *stabSettings = StabilizationSettings::GetInstance(m_uavoManager);
Q_ASSERT(stabSettings);
StabilizationSettings defaultSettings;
stabSettings->setData(defaultSettings.getData());
addModifiedObject(stabSettings, tr("Writing stabilization settings"));
}
/**
* Apply the stabilization settings computed
*/
void ConfigAutotuneWidget::saveStabilization()
{
StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager());
Q_ASSERT(stabilizationSettings);
if(!stabilizationSettings)
return;
// Check the settings are reasonable, or if not have the
// user confirm they want to continue.
SystemIdent *systemIdent = SystemIdent::GetInstance(getObjectManager());
Q_ASSERT(systemIdent);
if(!systemIdent)
return;
if (approveSettings(systemIdent->getData()) == false)
return;
// Make sure to recompute in case the other stab settings changed since
// the last time
recomputeStabilization();
// Apply this data to the board
stabilizationSettings->setData(stabSettings);
stabilizationSettings->updated();
}
/**
* Called whenever the gain ratios or measured values
* are changed
*/
void ConfigAutotuneWidget::recomputeStabilization()
{
SystemIdent *systemIdent = SystemIdent::GetInstance(getObjectManager());
Q_ASSERT(systemIdent);
if(!systemIdent)
return;
StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager());
Q_ASSERT(stabilizationSettings);
if(!stabilizationSettings)
return;
SystemIdent::DataFields systemIdentData = systemIdent->getData();
stabSettings = stabilizationSettings->getData();
// These three parameters define the desired response properties
// - rate scale in the fraction of the natural speed of the system
// to strive for.
// - damp is the amount of damping in the system. higher values
// make oscillations less likely
// - ghf is the amount of high frequency gain and limits the influence
// of noise
const double ghf = m_autotune->rateNoise->value() / 1000.0;
const double damp = m_autotune->rateDamp->value() / 100.0;
double tau = exp(systemIdentData.Tau);
double beta_roll = systemIdentData.Beta[SystemIdent::BETA_ROLL];
double beta_pitch = systemIdentData.Beta[SystemIdent::BETA_PITCH];
double wn = 1/tau;
double tau_d = 0;
for (int i = 0; i < 30; i++) {
double tau_d_roll = (2*damp*tau*wn - 1)/(4*tau*damp*damp*wn*wn - 2*damp*wn - tau*wn*wn + exp(beta_roll)*ghf);
double tau_d_pitch = (2*damp*tau*wn - 1)/(4*tau*damp*damp*wn*wn - 2*damp*wn - tau*wn*wn + exp(beta_pitch)*ghf);
// Select the slowest filter property
tau_d = (tau_d_roll > tau_d_pitch) ? tau_d_roll : tau_d_pitch;
wn = (tau + tau_d) / (tau*tau_d) / (2 * damp + 2);
}
// Set the real pole position. The first pole is quite slow, which
// prevents the integral being too snappy and driving too much
// overshoot.
const double a = ((tau+tau_d) / tau / tau_d - 2 * damp * wn) / 20.0;
const double b = ((tau+tau_d) / tau / tau_d - 2 * damp * wn - a);
qDebug() << "ghf: " << ghf;
qDebug() << "wn: " << wn << "tau_d: " << tau_d;
qDebug() << "a: " << a << " b: " << b;
// Calculate the gain for the outer loop by approximating the
// inner loop as a single order lpf. Set the outer loop to be
// critically damped;
const double zeta_o = 1.3;
const double kp_o = 1 / 4.0 / (zeta_o * zeta_o) / (1/wn);
// For now just run over roll and pitch
for (int i = 0; i < 2; i++) {
double beta = exp(systemIdentData.Beta[i]);
double ki = a * b * wn * wn * tau * tau_d / beta;
double kp = tau * tau_d * ((a+b)*wn*wn + 2*a*b*damp*wn) / beta - ki*tau_d;
double kd = (tau * tau_d * (a*b + wn*wn + (a+b)*2*damp*wn) - 1) / beta - kp * tau_d;
switch(i) {
case 0: // Roll
stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KP] = kp;
stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KI] = ki;
stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KD] = kd;
stabSettings.RollPI[StabilizationSettings::ROLLPI_KP] = kp_o;
stabSettings.RollPI[StabilizationSettings::ROLLPI_KI] = 0;
break;
case 1: // Pitch
stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KP] = kp;
stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI] = ki;
stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KD] = kd;
stabSettings.PitchPI[StabilizationSettings::PITCHPI_KP] = kp_o;
stabSettings.PitchPI[StabilizationSettings::PITCHPI_KI] = 0;
break;
}
}
stabSettings.DerivativeCutoff = 1 / (2*M_PI*tau_d);
// Display these computed settings
m_autotune->rollRateKp->setText(QString::number(stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KP]));
m_autotune->rollRateKi->setText(QString::number(stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KI]));
m_autotune->rollRateKd->setText(QString::number(stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KD]));
m_autotune->pitchRateKp->setText(QString::number(stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KP]));
m_autotune->pitchRateKi->setText(QString::number(stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI]));
m_autotune->pitchRateKd->setText(QString::number(stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KD]));
m_autotune->lblOuterKp->setText(QString::number(stabSettings.RollPI[StabilizationSettings::ROLLPI_KP]));
m_autotune->derivativeCutoff->setText(QString::number(stabSettings.DerivativeCutoff));
m_autotune->rollTau->setText(QString::number(tau,'g',3));
m_autotune->pitchTau->setText(QString::number(tau,'g',3));
m_autotune->wn->setText(QString::number(wn / 2 / M_PI, 'f', 1));
m_autotune->lblDamp->setText(QString::number(damp, 'g', 2));
m_autotune->lblNoise->setText(QString::number(ghf * 100, 'g', 2) + " %");
}
void ConfigStabilizationWidget::setupStabBanksGUI()
{
StabilizationSettings *stabSettings = qobject_cast<StabilizationSettings *>(getObject("StabilizationSettings"));
Q_ASSERT(stabSettings);
m_stabSettingsBankCount = stabSettings->getField("FlightModeMap")->getOptions().count();
// Set up fake tab widget stuff for pid banks support
m_stabTabBars.append(ui->basicPIDBankTabBar);
m_stabTabBars.append(ui->advancedPIDBankTabBar);
QAction *defaultStabMenuAction = new QAction(QIcon(":configgadget/images/gear.png"), QString(), this);
QAction *restoreAllAction = new QAction(tr("all to saved"), this);
connect(restoreAllAction, SIGNAL(triggered()), this, SLOT(restoreAllStabBanks()));
QAction *resetAllAction = new QAction(tr("all to default"), this);
connect(resetAllAction, SIGNAL(triggered()), this, SLOT(resetAllStabBanks()));
QAction *restoreCurrentAction = new QAction(tr("to saved"), this);
connect(restoreCurrentAction, SIGNAL(triggered()), this, SLOT(restoreCurrentAction()));
QAction *resetCurrentAction = new QAction(tr("to default"), this);
connect(resetCurrentAction, SIGNAL(triggered()), this, SLOT(resetCurrentStabBank()));
QAction *copyCurrentAction = new QAction(tr("to others"), this);
connect(copyCurrentAction, SIGNAL(triggered()), this, SLOT(copyCurrentStabBank()));
connect(&m_stabSettingsCopyFromSignalMapper, SIGNAL(mapped(int)), this, SLOT(copyFromBankToCurrent(int)));
connect(&m_stabSettingsCopyToSignalMapper, SIGNAL(mapped(int)), this, SLOT(copyToBankFromCurrent(int)));
connect(&m_stabSettingsSwapSignalMapper, SIGNAL(mapped(int)), this, SLOT(swapBankAndCurrent(int)));
foreach(QTabBar * tabBar, m_stabTabBars) {
for (int i = 0; i < m_stabSettingsBankCount; i++) {
tabBar->addTab(tr("Settings Bank %1").arg(i + 1));
tabBar->setTabData(i, QString("StabilizationSettingsBank%1").arg(i + 1));
QToolButton *tabButton = new QToolButton();
connect(this, SIGNAL(enableControlsChanged(bool)), tabButton, SLOT(setEnabled(bool)));
tabButton->setDefaultAction(defaultStabMenuAction);
tabButton->setAutoRaise(true);
tabButton->setPopupMode(QToolButton::InstantPopup);
tabButton->setToolTip(tr("The functions in this menu effect all fields in the settings banks,\n"
"not only the ones visible on screen."));
QMenu *tabMenu = new QMenu();
QMenu *restoreMenu = new QMenu(tr("Restore"));
QMenu *resetMenu = new QMenu(tr("Reset"));
QMenu *copyMenu = new QMenu(tr("Copy"));
QMenu *swapMenu = new QMenu(tr("Swap"));
QAction *menuAction;
for (int j = 0; j < m_stabSettingsBankCount; j++) {
if (j == i) {
restoreMenu->addAction(restoreCurrentAction);
resetMenu->addAction(resetCurrentAction);
copyMenu->addAction(copyCurrentAction);
} else {
menuAction = new QAction(tr("from %1").arg(j + 1), this);
connect(menuAction, SIGNAL(triggered()), &m_stabSettingsCopyFromSignalMapper, SLOT(map()));
m_stabSettingsCopyFromSignalMapper.setMapping(menuAction, j);
copyMenu->addAction(menuAction);
menuAction = new QAction(tr("to %1").arg(j + 1), this);
connect(menuAction, SIGNAL(triggered()), &m_stabSettingsCopyToSignalMapper, SLOT(map()));
m_stabSettingsCopyToSignalMapper.setMapping(menuAction, j);
copyMenu->addAction(menuAction);
menuAction = new QAction(tr("with %1").arg(j + 1), this);
connect(menuAction, SIGNAL(triggered()), &m_stabSettingsSwapSignalMapper, SLOT(map()));
m_stabSettingsSwapSignalMapper.setMapping(menuAction, j);
swapMenu->addAction(menuAction);
}
}
restoreMenu->addAction(restoreAllAction);
resetMenu->addAction(resetAllAction);
tabMenu->addMenu(copyMenu);
tabMenu->addMenu(swapMenu);
tabMenu->addMenu(resetMenu);
tabMenu->addMenu(restoreMenu);
tabButton->setMenu(tabMenu);
tabBar->setTabButton(i, QTabBar::RightSide, tabButton);
}
tabBar->setExpanding(false);
connect(tabBar, SIGNAL(currentChanged(int)), this, SLOT(stabBankChanged(int)));
}
for (int i = 0; i < m_stabSettingsBankCount; i++) {
if (i > 0) {
m_stabilizationObjectsString.append(",");
}
m_stabilizationObjectsString.append(m_stabTabBars.at(0)->tabData(i).toString());
}
}
