UASControlWidget.cc 8.79 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
/*=====================================================================

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 <QProcess>
#include <QPalette>

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

44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65
static struct full_mode_s modes_list_common[] = {
    { MAV_MODE_FLAG_MANUAL_INPUT_ENABLED,
            0 },
    { (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED),
            0 },
    { (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
            0 },
    { (MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
            0 },
};

static struct full_mode_s modes_list_px4[] = {
    { (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
            PX4_CUSTOM_MODE_MANUAL },
    { (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED),
            PX4_CUSTOM_MODE_SEATBELT },
    { (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
            PX4_CUSTOM_MODE_EASY },
    { (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
            PX4_CUSTOM_MODE_AUTO },
};

66
UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
67 68 69 70 71
    uasID(-1),
    modesList(NULL),
    modesNum(0),
    modeIdx(0),
    armed(false)
72 73 74
{
    ui.setupUi(this);

75 76
    this->setUAS(UASManager::instance()->getActiveUAS());

77 78 79 80
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
    connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int)));
    connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode()));

81 82
    ui.gridLayout->setAlignment(Qt::AlignTop);
}
83

84 85 86 87 88 89 90 91 92 93
void UASControlWidget::updateModesList()
{
    // Detect autopilot type
    int autopilot = 0;
    if (this->uasID >= 0) {
        UASInterface *uas = UASManager::instance()->getUASForId(this->uasID);
        if (uas) {
            autopilot = UASManager::instance()->getUASForId(this->uasID)->getAutopilotType();
        }
    }
94

95 96 97 98 99 100 101 102 103 104 105 106 107 108 109
    // Use corresponding modes list
    if (autopilot == MAV_AUTOPILOT_PX4) {
        modesList = modes_list_px4;
        modesNum = sizeof(modes_list_px4) / sizeof(struct full_mode_s);
    } else {
        modesList = modes_list_common;
        modesNum = sizeof(modes_list_common) / sizeof(struct full_mode_s);
    }

    // Set combobox items
    ui.modeComboBox->clear();
    for (int i = 0; i < modesNum; i++) {
        struct full_mode_s mode = modesList[i];
        ui.modeComboBox->insertItem(i, UAS::getShortModeTextFor(mode.baseMode, mode.customMode, autopilot).remove(0, 2), i);
    }
110

111 112 113 114
    // Select first mode in list
    modeIdx = 0;
    ui.modeComboBox->setCurrentIndex(modeIdx);
    ui.modeComboBox->update();
115 116 117 118
}

void UASControlWidget::setUAS(UASInterface* uas)
{
119
    if (this->uasID > 0) {
120
        UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uasID);
121 122 123 124 125 126 127 128 129
        if (oldUAS) {
            disconnect(ui.controlButton, SIGNAL(clicked()), oldUAS, SLOT(armSystem()));
            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(oldUAS, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int, QString, QString)));
            disconnect(oldUAS, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));
        }
130 131 132
    }

    // Connect user interface controls
133 134 135 136 137 138 139 140 141 142 143 144 145
    if (uas) {
        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->uasID = uas->getUASID();
        setBackgroundColor(uas->getColor());
146 147 148 149

        this->updateModesList();
        this->updateArmText();

150 151 152
    } else {
        this->uasID = -1;
    }
153 154 155 156 157 158 159
}

UASControlWidget::~UASControlWidget()
{

}

160
void UASControlWidget::updateArmText()
161
{
162
    if (armed) {
163
        ui.controlButton->setText(tr("DISARM SYSTEM"));
164
    } else {
165
        ui.controlButton->setText(tr("ARM SYSTEM"));
166 167 168 169 170 171 172 173 174 175 176 177 178 179
    }
}

/**
 * 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";
180
    uasColor = uasColor.darker(400);
181 182 183 184 185 186 187 188 189 190
    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);
}


191
void UASControlWidget::updateMode(int uas, QString mode, QString description)
192 193 194 195 196 197 198 199
{
    Q_UNUSED(uas);
    Q_UNUSED(mode);
    Q_UNUSED(description);
}

void UASControlWidget::updateState(int state)
{
200
    switch (state) {
201
    case (int)MAV_STATE_ACTIVE:
202
        armed = true;
203 204
        break;
    case (int)MAV_STATE_STANDBY:
205
        armed = false;
206 207
        break;
    }
208
    this->updateArmText();
209 210
}

211 212 213
/**
 * Called by the button
 */
214 215 216
void UASControlWidget::setMode(int mode)
{
    // Adapt context button mode
217
    modeIdx = mode;
218 219 220
    ui.modeComboBox->blockSignals(true);
    ui.modeComboBox->setCurrentIndex(mode);
    ui.modeComboBox->blockSignals(false);
221 222

    emit changedMode(mode);
223 224 225 226
}

void UASControlWidget::transmitMode()
{
227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242
    UASInterface* uas = UASManager::instance()->getUASForId(this->uasID);
    if (uas) {
        if (modeIdx >= 0 && modeIdx < modesNum) {
            struct full_mode_s mode = modesList[modeIdx];
            // include armed state
            if (armed) {
                mode.baseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
            } else {
                mode.baseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
            }

            uas->setMode(mode.baseMode, mode.customMode);
            QString modeText = ui.modeComboBox->currentText();

            ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(modeText).arg(uas->getUASName()));
        }
243 244 245 246 247
    }
}

void UASControlWidget::cycleContextButton()
{
248 249 250 251 252
    UAS* uas = dynamic_cast<UAS*>(UASManager::instance()->getUASForId(this->uasID));
    if (uas) {
        if (!armed) {
            uas->armSystem();
            ui.lastActionLabel->setText(QString("Arm %1").arg(uas->getUASName()));
253
        } else {
254 255
            uas->disarmSystem();
            ui.lastActionLabel->setText(QString("Disarm %1").arg(uas->getUASName()));
256 257 258 259
        }
    }
}