ArduCopterPidConfig.cc 9.51 KB
Newer Older
1 2
#include "ArduCopterPidConfig.h"

3
ArduCopterPidConfig::ArduCopterPidConfig(QWidget *parent) : AP2ConfigWidget(parent)
4
{
5
    ui.setupUi(this);
6 7 8 9 10
    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)));
11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53
    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;
54 55 56

    connect(ui.writePushButton,SIGNAL(clicked()),this,SLOT(writeButtonClicked()));
    connect(ui.refreshPushButton,SIGNAL(clicked()),this,SLOT(refreshButtonClicked()));
57 58 59



60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94
    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++)
95
    {
96
        ui.ch6OptComboBox->addItem(m_ch6ValueToTextList[i].second);
97 98
    }

99 100 101 102 103 104 105 106 107 108 109 110
    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++)
111
    {
112 113
        ui.ch7OptComboBox->addItem(m_ch78ValueToTextList[i].second);
        ui.ch8OptComboBox->addItem(m_ch78ValueToTextList[i].second);
114
    }
115
    initConnections();
116
}
117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135
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)));
    }
}
136 137 138 139

ArduCopterPidConfig::~ArduCopterPidConfig()
{
}
140 141
void ArduCopterPidConfig::parameterChanged(int uas, int component, QString parameterName, QVariant value)
{
142
    if (m_nameToBoxMap.contains(parameterName))
143
    {
144
        m_nameToBoxMap[parameterName]->setValue(value.toDouble());
145
    }
146 147
    else if (parameterName == "TUNE")
    {
148
        for (int i=0;i<m_ch6ValueToTextList.size();i++)
149
        {
150
            if (m_ch6ValueToTextList[i].first == value.toInt())
151 152 153 154 155 156 157
            {
                ui.ch6OptComboBox->setCurrentIndex(i);
            }
        }
    }
    else if (parameterName == "CH7_OPT")
    {
158
        for (int i=0;i<m_ch78ValueToTextList.size();i++)
159
        {
160
            if (m_ch78ValueToTextList[i].first == value.toInt())
161 162 163 164 165 166 167
            {
                ui.ch7OptComboBox->setCurrentIndex(i);
            }
        }
    }
    else if (parameterName == "CH8_OPT")
    {
168
        for (int i=0;i<m_ch78ValueToTextList.size();i++)
169
        {
170
            if (m_ch78ValueToTextList[i].first == value.toInt())
171 172
            {
                ui.ch8OptComboBox->setCurrentIndex(i);
173
            }
174 175
        }
    }
176 177 178 179 180
}
void ArduCopterPidConfig::writeButtonClicked()
{
    if (!m_uas)
    {
181
        showNullMAVErrorMessageBox();
182 183
        return;
    }
184
    for (QMap<QString,QDoubleSpinBox*>::const_iterator i=m_nameToBoxMap.constBegin();i!=m_nameToBoxMap.constEnd();i++)
185 186 187
    {
        m_uas->getParamManager()->setParameter(1,i.key(),i.value()->value());
    }
188 189 190
    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);
191 192 193 194 195 196
}

void ArduCopterPidConfig::refreshButtonClicked()
{
    if (!m_uas)
    {
197
        showNullMAVErrorMessageBox();
198 199
        return;
    }
200
    for (QMap<QString,QDoubleSpinBox*>::const_iterator i=m_nameToBoxMap.constBegin();i!=m_nameToBoxMap.constEnd();i++)
201 202 203
    {
        m_uas->getParamManager()->requestParameterUpdate(1,i.key());
    }
204 205 206
    m_uas->getParamManager()->requestParameterUpdate(1,"TUNE");
    m_uas->getParamManager()->requestParameterUpdate(1,"CH7_OPT");
    m_uas->getParamManager()->requestParameterUpdate(1,"CH8_OPT");
207
}