zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
地面站终端 App
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
3496 Commits
2 Branches
0 Tags
363 MiB
Tree: 1518d2064b
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '1518d2064b'
${ noResults }
QG4/src/ui/configuration/ArduCopterPidConfig.cc

210 lines
9.6 KiB
Raw Blame History

#include "ArduCopterPidConfig.h"
ArduCopterPidConfig::ArduCopterPidConfig(QWidget *parent) : AP2ConfigWidget(parent)
{
ui.setupUi(this);
m_pitchRollLocked = false;
connect(ui.checkBox,SIGNAL(clicked(bool)),this,SLOT(lockCheckBoxClicked(bool)));
connect(ui.stabilPitchPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double)));
connect(ui.stabilRollPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double)));
connect(ui.stabilYawPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double)));
m_nameToBoxMap["STB_RLL_P"] = ui.stabilPitchPSpinBox;
m_nameToBoxMap["STB_PIT_P"] = ui.stabilRollPSpinBox;
m_nameToBoxMap["STB_YAW_P"] = ui.stabilYawPSpinBox;
m_nameToBoxMap["HLD_LAT_P"] = ui.loiterPidPSpinBox;
m_nameToBoxMap["RATE_RLL_P"] = ui.rateRollPSpinBox;
m_nameToBoxMap["RATE_RLL_I"] = ui.rateRollISpinBox;
m_nameToBoxMap["RATE_RLL_D"] = ui.rateRollDSpinBox;
m_nameToBoxMap["RATE_RLL_IMAX"] = ui.rateRollIMAXSpinBox;
m_nameToBoxMap["RATE_PIT_P"] = ui.ratePitchPSpinBox;
m_nameToBoxMap["RATE_PIT_I"] = ui.ratePitchISpinBox;
m_nameToBoxMap["RATE_PIT_D"] = ui.ratePitchDSpinBox;
m_nameToBoxMap["RATE_PIT_IMAX"] = ui.ratePitchIMAXSpinBox;
m_nameToBoxMap["RATE_YAW_P"] = ui.rateYawPSpinBox;
m_nameToBoxMap["RATE_YAW_I"] = ui.rateYawISpinBox;
m_nameToBoxMap["RATE_YAW_D"] = ui.rateYawDSpinBox;
m_nameToBoxMap["RATE_YAW_IMAX"] = ui.rateYawIMAXSpinBox;
m_nameToBoxMap["LOITER_LAT_P"] = ui.rateLoiterPSpinBox;
m_nameToBoxMap["LOITER_LAT_I"] = ui.rateLoiterISpinBox;
m_nameToBoxMap["LOITER_LAT_D"] = ui.rateLoiterDSpinBox;
m_nameToBoxMap["LOITER_LAT_IMAX"] = ui.rateLoiterIMAXSpinBox;
m_nameToBoxMap["THR_ACCEL_P"] = ui.throttleAccelPSpinBox;
m_nameToBoxMap["THR_ACCEL_I"] = ui.throttleAccelISpinBox;
m_nameToBoxMap["THR_ACCEL_D"] = ui.throttleAccelDSpinBox;
m_nameToBoxMap["THR_ACCEL_IMAX"] = ui.throttleAccelIMAXSpinBox;
m_nameToBoxMap["THR_RATE_P"] = ui.throttleRatePSpinBox;
m_nameToBoxMap["THR_RATE_D"] = ui.throttleDateDSpinBox;
m_nameToBoxMap["THR_ALT_P"] = ui.altitudeHoldPSpinBox;
m_nameToBoxMap["WPNAV_SPEED"] = ui.wpNavLoiterSpeedSpinBox;
m_nameToBoxMap["WPNAV_RADIUS"] = ui.wpNavRadiusSpinBox;
m_nameToBoxMap["WPNAV_SPEED_DN"] = ui.wpNavSpeedDownSpinBox;
m_nameToBoxMap["WPNAV_LOIT_SPEED"] = ui.wpNavSpeedSpinBox;
m_nameToBoxMap["WPNAV_SPEED_UP"] = ui.wpNavSpeedUpSpinBox;
m_nameToBoxMap["TUNE_HIGH"] = ui.ch6MaxSpinBox;
m_nameToBoxMap["TUNE_LOW"] = ui.ch6MinSpinBox;
connect(ui.writePushButton,SIGNAL(clicked()),this,SLOT(writeButtonClicked()));
connect(ui.refreshPushButton,SIGNAL(clicked()),this,SLOT(refreshButtonClicked()));
m_ch6ValueToTextList.append(QPair<int,QString>(0,"CH6_NONE"));
m_ch6ValueToTextList.append(QPair<int,QString>(1,"CH6_STABILIZE_KP"));
m_ch6ValueToTextList.append(QPair<int,QString>(2,"CH6_STABILIZE_KI"));
m_ch6ValueToTextList.append(QPair<int,QString>(3,"CH6_YAW_KP"));
m_ch6ValueToTextList.append(QPair<int,QString>(24,"CH6_YAW_KI"));
m_ch6ValueToTextList.append(QPair<int,QString>(4,"CH6_RATE_KP"));
m_ch6ValueToTextList.append(QPair<int,QString>(5,"CH6_RATE_KI"));
m_ch6ValueToTextList.append(QPair<int,QString>(6,"CH6_YAW_RATE_KP"));
m_ch6ValueToTextList.append(QPair<int,QString>(26,"CH6_YAW_RATE_KD"));
m_ch6ValueToTextList.append(QPair<int,QString>(7,"CH6_THROTTLE_KP"));
m_ch6ValueToTextList.append(QPair<int,QString>(9,"CH6_RELAY"));
m_ch6ValueToTextList.append(QPair<int,QString>(10,"CH6_WP_SPEED"));
m_ch6ValueToTextList.append(QPair<int,QString>(12,"CH6_LOITER_KP"));
m_ch6ValueToTextList.append(QPair<int,QString>(13,"CH6_HELI_EXTERNAL_GYRO"));
m_ch6ValueToTextList.append(QPair<int,QString>(14,"CH6_THR_HOLD_KP"));
m_ch6ValueToTextList.append(QPair<int,QString>(17,"CH6_OPTFLOW_KP"));
m_ch6ValueToTextList.append(QPair<int,QString>(18,"CH6_OPTFLOW_KI"));
m_ch6ValueToTextList.append(QPair<int,QString>(19,"CH6_OPTFLOW_KD"));
m_ch6ValueToTextList.append(QPair<int,QString>(21,"CH6_RATE_KD"));
m_ch6ValueToTextList.append(QPair<int,QString>(22,"CH6_LOITER_RATE_KP"));
m_ch6ValueToTextList.append(QPair<int,QString>(23,"CH6_LOITER_RATE_KD"));
m_ch6ValueToTextList.append(QPair<int,QString>(25,"CH6_ACRO_KP"));
m_ch6ValueToTextList.append(QPair<int,QString>(27,"CH6_LOITER_KI"));
m_ch6ValueToTextList.append(QPair<int,QString>(28,"CH6_LOITER_RATE_KI"));
m_ch6ValueToTextList.append(QPair<int,QString>(29,"CH6_STABILIZE_KD"));
m_ch6ValueToTextList.append(QPair<int,QString>(30,"CH6_AHRS_YAW_KP"));
m_ch6ValueToTextList.append(QPair<int,QString>(31,"CH6_AHRS_KP"));
m_ch6ValueToTextList.append(QPair<int,QString>(32,"CH6_INAV_TC"));
m_ch6ValueToTextList.append(QPair<int,QString>(33,"CH6_THROTTLE_KI"));
m_ch6ValueToTextList.append(QPair<int,QString>(34,"CH6_THR_ACCEL_KP"));
m_ch6ValueToTextList.append(QPair<int,QString>(35,"CH6_THR_ACCEL_KI"));
m_ch6ValueToTextList.append(QPair<int,QString>(36,"CH6_THR_ACCEL_KD"));
m_ch6ValueToTextList.append(QPair<int,QString>(38,"CH6_DECLINATION"));
m_ch6ValueToTextList.append(QPair<int,QString>(39,"CH6_CIRCLE_RATE"));
for (int i=0;i<m_ch6ValueToTextList.size();i++)
{
ui.ch6OptComboBox->addItem(m_ch6ValueToTextList[i].second);
}
m_ch78ValueToTextList.append(QPair<int,QString>(0,"Do nothing"));
m_ch78ValueToTextList.append(QPair<int,QString>(2,"Flip"));
m_ch78ValueToTextList.append(QPair<int,QString>(3,"Simple mode"));
m_ch78ValueToTextList.append(QPair<int,QString>(4,"RTL"));
m_ch78ValueToTextList.append(QPair<int,QString>(5,"Save Trim"));
m_ch78ValueToTextList.append(QPair<int,QString>(7,"Save WP"));
m_ch78ValueToTextList.append(QPair<int,QString>(8,"Multi Mode"));
m_ch78ValueToTextList.append(QPair<int,QString>(9,"Camera Trigger"));
m_ch78ValueToTextList.append(QPair<int,QString>(10,"Sonar"));
m_ch78ValueToTextList.append(QPair<int,QString>(11,"Fence"));
m_ch78ValueToTextList.append(QPair<int,QString>(12,"ResetToArmedYaw"));
for (int i=0;i<m_ch78ValueToTextList.size();i++)
{
ui.ch7OptComboBox->addItem(m_ch78ValueToTextList[i].second);
ui.ch8OptComboBox->addItem(m_ch78ValueToTextList[i].second);
}
initConnections();
}
void ArduCopterPidConfig::lockCheckBoxClicked(bool checked)
{
m_pitchRollLocked = checked;
}
void ArduCopterPidConfig::stabilLockedChanged(double value)
{
if (m_pitchRollLocked)
{
disconnect(ui.stabilPitchPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double)));
disconnect(ui.stabilRollPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double)));
disconnect(ui.stabilYawPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double)));
ui.stabilPitchPSpinBox->setValue(value);
ui.stabilRollPSpinBox->setValue(value);
ui.stabilYawPSpinBox->setValue(value);
connect(ui.stabilPitchPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double)));
connect(ui.stabilRollPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double)));
connect(ui.stabilYawPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double)));
}
}
ArduCopterPidConfig::~ArduCopterPidConfig()
{
}
void ArduCopterPidConfig::parameterChanged(int uas, int component, QString parameterName, QVariant value)
{
Q_UNUSED(uas);
Q_UNUSED(component);
if (m_nameToBoxMap.contains(parameterName))
{
m_nameToBoxMap[parameterName]->setValue(value.toDouble());
}
else if (parameterName == "TUNE")
{
for (int i=0;i<m_ch6ValueToTextList.size();i++)
{
if (m_ch6ValueToTextList[i].first == value.toInt())
{
ui.ch6OptComboBox->setCurrentIndex(i);
}
}
}
else if (parameterName == "CH7_OPT")
{
for (int i=0;i<m_ch78ValueToTextList.size();i++)
{
if (m_ch78ValueToTextList[i].first == value.toInt())
{
ui.ch7OptComboBox->setCurrentIndex(i);
}
}
}
else if (parameterName == "CH8_OPT")
{
for (int i=0;i<m_ch78ValueToTextList.size();i++)
{
if (m_ch78ValueToTextList[i].first == value.toInt())
{
ui.ch8OptComboBox->setCurrentIndex(i);
}
}
}
}
void ArduCopterPidConfig::writeButtonClicked()
{
if (!m_uas)
{
showNullMAVErrorMessageBox();
return;
}
for (QMap<QString,QDoubleSpinBox*>::const_iterator i=m_nameToBoxMap.constBegin();i!=m_nameToBoxMap.constEnd();i++)
{
m_uas->getParamManager()->setParameter(1,i.key(),i.value()->value());
}
m_uas->getParamManager()->setParameter(1,"TUNE",m_ch78ValueToTextList[ui.ch6OptComboBox->currentIndex()].first);
m_uas->getParamManager()->setParameter(1,"CH7_OPT",m_ch78ValueToTextList[ui.ch7OptComboBox->currentIndex()].first);
m_uas->getParamManager()->setParameter(1,"CH8_OPT",m_ch78ValueToTextList[ui.ch8OptComboBox->currentIndex()].first);
}
void ArduCopterPidConfig::refreshButtonClicked()
{
if (!m_uas)
{
showNullMAVErrorMessageBox();
return;
}
for (QMap<QString,QDoubleSpinBox*>::const_iterator i=m_nameToBoxMap.constBegin();i!=m_nameToBoxMap.constEnd();i++)
{
m_uas->getParamManager()->requestParameterUpdate(1,i.key());
}
m_uas->getParamManager()->requestParameterUpdate(1,"TUNE");
m_uas->getParamManager()->requestParameterUpdate(1,"CH7_OPT");
m_uas->getParamManager()->requestParameterUpdate(1,"CH8_OPT");
}