UASControlWidget.cc 7.96 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 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
/*=====================================================================

PIXHAWK Micro Air Vehicle Flying Robotics Toolkit

(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>

This file is part of the PIXHAWK project

    PIXHAWK is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    PIXHAWK is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Definition of widget controlling one MAV
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QString>
#include <QTimer>
#include <QLabel>
#include <QFileDialog>
#include <QDebug>
#include <QProcess>
#include <QPalette>

#include "UASControlWidget.h"
#include <UASManager.h>
#include <UAS.h>
#include "QGC.h"

UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
46
    uas(0),
47
    uasMode(0),
48
    engineOn(false)
49 50 51 52 53
{
    ui.setupUi(this);

    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
    ui.modeComboBox->clear();
Lorenz Meier's avatar
Lorenz Meier committed
54 55 56 57 58 59
    ui.modeComboBox->insertItem(0, UAS::getShortModeTextFor(MAV_MODE_PREFLIGHT).remove(0, 2), MAV_MODE_PREFLIGHT);
    ui.modeComboBox->insertItem(1, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED));
    ui.modeComboBox->insertItem(2, UAS::getShortModeTextFor(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED).remove(0, 2), MAV_MODE_FLAG_MANUAL_INPUT_ENABLED);
    ui.modeComboBox->insertItem(3, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED));
    ui.modeComboBox->insertItem(4, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED));
    ui.modeComboBox->insertItem(5, UAS::getShortModeTextFor((MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED));
60 61 62
    connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int)));
    connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode()));

63 64
    uasMode = ui.modeComboBox->itemData(ui.modeComboBox->currentIndex()).toInt();

65 66 67
    ui.modeComboBox->setCurrentIndex(0);

    ui.gridLayout->setAlignment(Qt::AlignTop);
68

69 70 71 72
}

void UASControlWidget::setUAS(UASInterface* uas)
{
73 74
    if (this->uas != 0)
    {
75
        UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uas);
76
        disconnect(ui.controlButton, SIGNAL(clicked()), oldUAS, SLOT(armSystem()));
77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107
        disconnect(ui.liftoffButton, SIGNAL(clicked()), oldUAS, SLOT(launch()));
        disconnect(ui.landButton, SIGNAL(clicked()), oldUAS, SLOT(home()));
        disconnect(ui.shutdownButton, SIGNAL(clicked()), oldUAS, SLOT(shutdown()));
        //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition()));
        disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
        disconnect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));
    }

    // Connect user interface controls
    connect(ui.controlButton, SIGNAL(clicked()), this, SLOT(cycleContextButton()));
    connect(ui.liftoffButton, SIGNAL(clicked()), uas, SLOT(launch()));
    connect(ui.landButton, SIGNAL(clicked()), uas, SLOT(home()));
    connect(ui.shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown()));
    //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition()));
    connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
    connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));

    ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName());

    this->uas = uas->getUASID();
    setBackgroundColor(uas->getColor());
}

UASControlWidget::~UASControlWidget()
{

}

void UASControlWidget::updateStatemachine()
{

108 109
    if (engineOn)
    {
110
        ui.controlButton->setText(tr("DISARM SYSTEM"));
111 112 113
    }
    else
    {
114
        ui.controlButton->setText(tr("ARM SYSTEM"));
115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148
    }
}

/**
 * Set the background color based on the MAV color. If the MAV is selected as the
 * currently actively controlled system, the frame color is highlighted
 */
void UASControlWidget::setBackgroundColor(QColor color)
{
    // UAS color
    QColor uasColor = color;
    QString colorstyle;
    QString borderColor = "#4A4A4F";
    borderColor = "#FA4A4F";
    uasColor = uasColor.darker(900);
    colorstyle = colorstyle.sprintf("QLabel { border-radius: 3px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 0px solid %s; }",
                                    uasColor.red(), uasColor.green(), uasColor.blue(), borderColor.toStdString().c_str());
    setStyleSheet(colorstyle);
    QPalette palette = this->palette();
    palette.setBrush(QPalette::Window, QBrush(uasColor));
    setPalette(palette);
    setAutoFillBackground(true);
}


void UASControlWidget::updateMode(int uas,QString mode,QString description)
{
    Q_UNUSED(uas);
    Q_UNUSED(mode);
    Q_UNUSED(description);
}

void UASControlWidget::updateState(int state)
{
149 150
    switch (state)
    {
151 152
    case (int)MAV_STATE_ACTIVE:
        engineOn = true;
153
        ui.controlButton->setText(tr("DISARM SYSTEM"));
154 155 156
        break;
    case (int)MAV_STATE_STANDBY:
        engineOn = false;
157
        ui.controlButton->setText(tr("ARM SYSTEM"));
158 159 160 161
        break;
    }
}

162 163 164
/**
 * Called by the button
 */
165 166 167
void UASControlWidget::setMode(int mode)
{
    // Adapt context button mode
pixhawk's avatar
pixhawk committed
168
    uasMode = ui.modeComboBox->itemData(mode).toInt();
169 170 171
    ui.modeComboBox->blockSignals(true);
    ui.modeComboBox->setCurrentIndex(mode);
    ui.modeComboBox->blockSignals(false);
172 173

    emit changedMode(mode);
174 175 176 177
}

void UASControlWidget::transmitMode()
{
178 179 180
    UASInterface* mav = UASManager::instance()->getUASForId(this->uas);
    if (mav)
    {
181 182 183 184 185 186
        // include armed state
        if (engineOn)
            uasMode |= MAV_MODE_FLAG_SAFETY_ARMED;
        else
            uasMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;

187
        mav->setMode(uasMode);
188 189 190
        QString mode = ui.modeComboBox->currentText();

        ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(mode).arg(mav->getUASName()));
191 192 193 194 195 196
    }
}

void UASControlWidget::cycleContextButton()
{
    UAS* mav = dynamic_cast<UAS*>(UASManager::instance()->getUASForId(this->uas));
197 198
    if (mav)
    {
199

200 201 202
        if (!engineOn)
        {
            mav->armSystem();
203
            ui.lastActionLabel->setText(QString("Enabled motors on %1").arg(mav->getUASName()));
204
        } else {
205
            mav->disarmSystem();
206 207 208 209 210 211 212 213 214 215 216 217
            ui.lastActionLabel->setText(QString("Disabled motors on %1").arg(mav->getUASName()));
        }
        // Update state now and in several intervals when MAV might have changed state
        updateStatemachine();

        QTimer::singleShot(50, this, SLOT(updateStatemachine()));
        QTimer::singleShot(200, this, SLOT(updateStatemachine()));

    }

}