UASControlWidget.cc 9.27 KB
Newer Older
pixhawk's avatar
pixhawk committed
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
/*=====================================================================

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>
38
#include <QPalette>
pixhawk's avatar
pixhawk committed
39 40 41 42 43 44

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

lm's avatar
lm committed
45 46 47 48 49
#define CONTROL_MODE_LOCKED "MODE LOCKED"
#define CONTROL_MODE_MANUAL "MODE MANUAL"
#define CONTROL_MODE_GUIDED "MODE GUIDED"
#define CONTROL_MODE_AUTO   "MODE AUTO"
#define CONTROL_MODE_TEST1  "MODE TEST1"
lm's avatar
lm committed
50 51
#define CONTROL_MODE_TEST2  "MODE TEST2"
#define CONTROL_MODE_TEST3  "MODE TEST3"
lm's avatar
lm committed
52 53
#define CONTROL_MODE_READY  "MODE TEST3"
#define CONTROL_MODE_RC_TRAINING  "MODE RC TRAINING"
lm's avatar
lm committed
54

pixhawk's avatar
pixhawk committed
55 56 57 58 59 60 61
#define CONTROL_MODE_LOCKED_INDEX 1
#define CONTROL_MODE_MANUAL_INDEX 2
#define CONTROL_MODE_GUIDED_INDEX 3
#define CONTROL_MODE_AUTO_INDEX   4
#define CONTROL_MODE_TEST1_INDEX  5
#define CONTROL_MODE_TEST2_INDEX  6
#define CONTROL_MODE_TEST3_INDEX  7
lm's avatar
lm committed
62 63
#define CONTROL_MODE_READY_INDEX  8
#define CONTROL_MODE_RC_TRAINING_INDEX  9
lm's avatar
lm committed
64

pixhawk's avatar
pixhawk committed
65
UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
66
uas(0),
67
engineOn(false)
pixhawk's avatar
pixhawk committed
68 69 70 71
{
    ui.setupUi(this);

    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
lm's avatar
lm committed
72
    ui.modeComboBox->clear();
lm's avatar
lm committed
73 74 75 76 77 78
    ui.modeComboBox->insertItem(0, "Select..");
    ui.modeComboBox->insertItem(CONTROL_MODE_LOCKED_INDEX, CONTROL_MODE_LOCKED);
    ui.modeComboBox->insertItem(CONTROL_MODE_MANUAL_INDEX, CONTROL_MODE_MANUAL);
    ui.modeComboBox->insertItem(CONTROL_MODE_GUIDED_INDEX, CONTROL_MODE_GUIDED);
    ui.modeComboBox->insertItem(CONTROL_MODE_AUTO_INDEX, CONTROL_MODE_AUTO);
    ui.modeComboBox->insertItem(CONTROL_MODE_TEST1_INDEX, CONTROL_MODE_TEST1);
lm's avatar
lm committed
79 80
    ui.modeComboBox->insertItem(CONTROL_MODE_TEST2_INDEX, CONTROL_MODE_TEST2);
    ui.modeComboBox->insertItem(CONTROL_MODE_TEST3_INDEX, CONTROL_MODE_TEST3);
lm's avatar
lm committed
81 82
    ui.modeComboBox->insertItem(CONTROL_MODE_READY_INDEX, CONTROL_MODE_READY);
    ui.modeComboBox->insertItem(CONTROL_MODE_RC_TRAINING_INDEX, CONTROL_MODE_RC_TRAINING);
83 84
    connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int)));
    connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode()));
lm's avatar
lm committed
85 86

    ui.modeComboBox->setCurrentIndex(0);
pixhawk's avatar
pixhawk committed
87 88 89 90
}

void UASControlWidget::setUAS(UASInterface* uas)
{
91
    if (this->uas != 0)
pixhawk's avatar
pixhawk committed
92
    {
93 94 95 96 97
        UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uas);
        disconnect(ui.controlButton, SIGNAL(clicked()), oldUAS, SLOT(enable_motors()));
        disconnect(ui.liftoffButton, SIGNAL(clicked()), oldUAS, SLOT(launch()));
        disconnect(ui.landButton, SIGNAL(clicked()), oldUAS, SLOT(home()));
        disconnect(ui.shutdownButton, SIGNAL(clicked()), oldUAS, SLOT(shutdown()));
98
        connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition()));
99 100
        disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
        disconnect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));
pixhawk's avatar
pixhawk committed
101 102
    }

103 104 105 106 107
    // 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()));
108
    connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition()));
109 110 111
    connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
    connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));

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

lm's avatar
lm committed
114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131
    // Check if additional controls should be loaded
    UAS* mav = dynamic_cast<UAS*>(uas);
    if (mav)
    {
        QPushButton* startRecButton = new QPushButton(tr("Record"));
        connect(startRecButton, SIGNAL(clicked()), mav, SLOT(startDataRecording()));
        ui.gridLayout->addWidget(startRecButton, 10, 1);

        QPushButton* pauseRecButton = new QPushButton(tr("Pause"));
        connect(pauseRecButton, SIGNAL(clicked()), mav, SLOT(pauseDataRecording()));
        ui.gridLayout->addWidget(pauseRecButton, 10, 2);

        QPushButton* stopRecButton = new QPushButton(tr("Stop"));
        connect(stopRecButton, SIGNAL(clicked()), mav, SLOT(stopDataRecording()));
        ui.gridLayout->addWidget(stopRecButton, 10, 3);
    }


132 133
    this->uas = uas->getUASID();
    setBackgroundColor(uas->getColor());
pixhawk's avatar
pixhawk committed
134 135
}

136 137
UASControlWidget::~UASControlWidget()
{
pixhawk's avatar
pixhawk committed
138 139 140

}

141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162
/**
 * 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);
}


163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184
void UASControlWidget::updateMode(int uas,QString mode,QString description)
{
    Q_UNUSED(uas);
    Q_UNUSED(mode);
    Q_UNUSED(description);
}

void UASControlWidget::updateState(int state)
{
    switch (state)
    {
    case (int)MAV_STATE_ACTIVE:
        engineOn = true;
        ui.controlButton->setText(tr("Stop Engine"));
        break;
    case (int)MAV_STATE_STANDBY:
        engineOn = false;
        ui.controlButton->setText(tr("Activate Engine"));
        break;
    }
}

pixhawk's avatar
pixhawk committed
185 186
void UASControlWidget::setMode(int mode)
{
187
    // Adapt context button mode
lm's avatar
lm committed
188
    if (mode == CONTROL_MODE_LOCKED_INDEX)
pixhawk's avatar
pixhawk committed
189
    {
lm's avatar
lm committed
190 191
        uasMode = (unsigned int)MAV_MODE_LOCKED;
        ui.modeComboBox->setCurrentIndex(mode);
pixhawk's avatar
pixhawk committed
192
    }
lm's avatar
lm committed
193 194 195 196 197 198
    else if (mode == CONTROL_MODE_MANUAL_INDEX)
    {
        uasMode = (unsigned int)MAV_MODE_MANUAL;
        ui.modeComboBox->setCurrentIndex(mode);
    }
    else if (mode == CONTROL_MODE_GUIDED_INDEX)
199
    {
lm's avatar
lm committed
200
        uasMode = (unsigned int)MAV_MODE_GUIDED;
201 202
        ui.modeComboBox->setCurrentIndex(mode);
    }
lm's avatar
lm committed
203 204 205 206 207 208 209 210 211 212
    else if (mode == CONTROL_MODE_AUTO_INDEX)
    {
        uasMode = (unsigned int)MAV_MODE_AUTO;
        ui.modeComboBox->setCurrentIndex(mode);
    }
    else if (mode == CONTROL_MODE_TEST1_INDEX)
    {
        uasMode = (unsigned int)MAV_MODE_TEST1;
        ui.modeComboBox->setCurrentIndex(mode);
    }
lm's avatar
lm committed
213 214 215 216 217 218 219 220 221 222
    else if (mode == CONTROL_MODE_TEST2_INDEX)
    {
        uasMode = (unsigned int)MAV_MODE_TEST2;
        ui.modeComboBox->setCurrentIndex(mode);
    }
    else if (mode == CONTROL_MODE_TEST3_INDEX)
    {
        uasMode = (unsigned int)MAV_MODE_TEST3;
        ui.modeComboBox->setCurrentIndex(mode);
    }
lm's avatar
lm committed
223 224 225 226 227
    else if (mode == CONTROL_MODE_RC_TRAINING_INDEX)
    {
        uasMode = (unsigned int)MAV_MODE_RC_TRAINING;
        ui.modeComboBox->setCurrentIndex(mode);
    }
lm's avatar
lm committed
228 229 230 231 232 233 234 235
    else
    {
        qDebug() << "ERROR! MODE NOT FOUND";
        uasMode = 0;
    }


    qDebug() << "SET MODE REQUESTED" << uasMode;
236 237 238 239
}

void UASControlWidget::transmitMode()
{
lm's avatar
lm committed
240 241
    if (uasMode != 0)
    {
242 243 244 245 246 247
        UASInterface* mav = UASManager::instance()->getUASForId(this->uas);
        if (mav)
        {
            mav->setMode(uasMode);
            ui.lastActionLabel->setText(QString("Set new mode for system %1").arg(mav->getUASName()));
        }
lm's avatar
lm committed
248
    }
pixhawk's avatar
pixhawk committed
249 250 251 252
}

void UASControlWidget::cycleContextButton()
{
253
    UAS* mav = dynamic_cast<UAS*>(UASManager::instance()->getUASForId(this->uas));
pixhawk's avatar
pixhawk committed
254 255 256
    if (mav)
    {

257
        if (!engineOn)
pixhawk's avatar
pixhawk committed
258 259 260
        {
            ui.controlButton->setText(tr("Stop Engine"));
            mav->enable_motors();
261
            ui.lastActionLabel->setText(QString("Attempted to enable motors on %1").arg(mav->getUASName()));
262 263 264
        }
        else
        {
pixhawk's avatar
pixhawk committed
265 266
            ui.controlButton->setText(tr("Activate Engine"));
            mav->disable_motors();
267
            ui.lastActionLabel->setText(QString("Attempted to disable motors on %1").arg(mav->getUASName()));
pixhawk's avatar
pixhawk committed
268
        }
269 270
        //ui.controlButton->setText(tr("Force Landing"));
        //ui.controlButton->setText(tr("KILL VEHICLE"));
pixhawk's avatar
pixhawk committed
271 272 273 274
    }

}