Newer
Older
/*=====================================================================
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
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#include <cmath>
#include <QDateTime>
#include <QDebug>
#include "MG.h"
#include "UASManager.h"
#include "UASView.h"
#include "ui_UASView.h"
UASView::UASView(UASInterface* uas, QWidget *parent) :
QWidget(parent),
startTime(0),
chargeLevel(0),
uas(uas),
load(0),
x(0),
y(0),
z(0),
totalSpeed(0),
lat(0),
lon(0),
alt(0),
groundDistance(0),
m_ui(new Ui::UASView)
{
m_ui->setupUi(this);
// Setup communication
//connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(receiveValue(int,QString,double,quint64)));
connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int)));
connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*)));
connect(uas, SIGNAL(thrustChanged(UASInterface*, double)), this, SLOT(updateThrust(UASInterface*, double)));
connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString,QString)));
connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double)));
//connect(uas, SIGNAL(waypointUpdated(int,int,double,double,double,double,bool,bool)), this, SLOT(setWaypoint(int,int,double,double,double,double,bool,bool)));
connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int,int)));
connect(&(uas->getWaypointManager()), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointUpdated(quint16)));
connect(uas, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
connect(UASManager::instance(), SIGNAL(activeUASStatusChanged(UASInterface*,bool)), this, SLOT(updateActiveUAS(UASInterface*,bool)));
connect(m_ui->uasViewFrame, SIGNAL(clicked(bool)), this, SLOT(setUASasActive(bool)));
// Setup user interaction
connect(m_ui->liftoffButton, SIGNAL(clicked()), uas, SLOT(launch()));
connect(m_ui->haltButton, SIGNAL(clicked()), uas, SLOT(halt()));
connect(m_ui->continueButton, SIGNAL(clicked()), uas, SLOT(go()));
connect(m_ui->landButton, SIGNAL(clicked()), uas, SLOT(home()));
connect(m_ui->abortButton, SIGNAL(clicked()), uas, SLOT(emergencySTOP()));
connect(m_ui->killButton, SIGNAL(clicked()), uas, SLOT(emergencyKILL()));
connect(m_ui->shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown()));
// Set static values
// Name
if (uas->getUASName() == "")
{
m_ui->nameLabel->setText(tr("UAS") + QString::number(uas->getUASID()));
}
else
{
m_ui->nameLabel->setText(uas->getUASName());
}
// Heartbeat fade
refreshTimer = new QTimer(this);
connect(refreshTimer, SIGNAL(timeout()), this, SLOT(refresh()));
refreshTimer->start(updateInterval);
/**
* 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 UASView::setBackgroundColor()
{
// UAS color
QColor uasColor = uas->getColor();
QString colorstyle;
QString borderColor = "#4A4A4F";
if (isActive)
{
borderColor = "#FA4A4F";
uasColor = uasColor.darker(475);
}
else
{
uasColor = uasColor.darker(675);
}
colorstyle = colorstyle.sprintf("QGroupBox { border-radius: 5px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 2px solid %s; }",
uasColor.red(), uasColor.green(), uasColor.blue(), borderColor.toStdString().c_str());
m_ui->uasViewFrame->setStyleSheet(colorstyle);
}
void UASView::setUASasActive(bool active)
if (active)
{
UASManager::instance()->setActiveUAS(this->uas);
}
}
void UASView::updateActiveUAS(UASInterface* uas, bool active)
{
if (uas == this->uas)
{
this->isActive = active;
setBackgroundColor();
}
void UASView::updateMode(int sysId, QString status, QString description)
{
Q_UNUSED(description);
if (sysId == this->uas->getUASID()) m_ui->modeLabel->setText(status);
}
void UASView::mouseDoubleClickEvent (QMouseEvent * event)
{
Q_UNUSED(event);
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
UASManager::instance()->setActiveUAS(uas);
qDebug() << __FILE__ << __LINE__ << "DOUBLECLICKED";
}
void UASView::enterEvent(QEvent* event)
{
if (event->MouseMove) emit uasInFocus(uas);
qDebug() << __FILE__ << __LINE__ << "IN FOCUS";
}
void UASView::leaveEvent(QEvent* event)
{
if (event->MouseMove) emit uasOutFocus(uas);
qDebug() << __FILE__ << __LINE__ << "OUT OF FOCUS";
}
void UASView::receiveHeartbeat(UASInterface* uas)
{
if (uas == this->uas)
{
refreshTimer->stop();
QString colorstyle;
heartbeatColor = QColor(20, 200, 20);
colorstyle = colorstyle.sprintf("QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X;}",
heartbeatColor.red(), heartbeatColor.green(), heartbeatColor.blue());
m_ui->heartbeatIcon->setStyleSheet(colorstyle);
m_ui->heartbeatIcon->setAutoFillBackground(true);
refreshTimer->start(updateInterval);
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
}
}
/**
* The current system type is represented through the system icon.
*
* @param uas Source system, has to be the same as this->uas
* @param systemType type ID, following the MAVLink system type conventions
* @see http://pixhawk.ethz.ch/software/mavlink
*/
void UASView::setSystemType(UASInterface* uas, unsigned int systemType)
{
if (uas == this->uas)
{
// Set matching icon
switch (systemType)
{
case 0:
m_ui->typeButton->setIcon(QIcon(":/images/mavs/generic.svg"));
break;
case 1:
m_ui->typeButton->setIcon(QIcon(":/images/mavs/fixed-wing.svg"));
break;
case 2:
m_ui->typeButton->setIcon(QIcon(":/images/mavs/quadrotor.svg"));
break;
case 3:
m_ui->typeButton->setIcon(QIcon(":/images/mavs/coaxial.svg"));
break;
case 4:
m_ui->typeButton->setIcon(QIcon(":/images/mavs/helicopter.svg"));
break;
case 5:
m_ui->typeButton->setIcon(QIcon(":/images/mavs/groundstation.svg"));
break;
default:
m_ui->typeButton->setIcon(QIcon(":/images/mavs/unknown.svg"));
break;
}
}
}
void UASView::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec)
{
Q_UNUSED(usec);
void UASView::updateGlobalPosition(UASInterface* uas, double lon, double lat, double alt, quint64 usec)
Q_UNUSED(uas);
Q_UNUSED(usec);
this->lon = lon;
this->lat = lat;
this->alt = alt;
}
void UASView::updateSpeed(UASInterface*, double x, double y, double z, quint64 usec)
{
Q_UNUSED(usec);
totalSpeed = sqrt((pow(x, 2) + pow(y, 2) + pow(z, 2)));
void UASView::currentWaypointUpdated(quint16 waypoint)
{
m_ui->waypointLabel->setText(tr("WP") + QString::number(waypoint));
}
void UASView::setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current)
{
Q_UNUSED(x);
Q_UNUSED(y);
Q_UNUSED(z);
Q_UNUSED(yaw);
Q_UNUSED(autocontinue);
if (uasId == this->uas->getUASID())
{
if (current)
{
m_ui->waypointLabel->setText(tr("WP") + QString::number(id));
}
}
}
void UASView::selectWaypoint(int uasId, int id)
{
if (uasId == this->uas->getUASID())
{
m_ui->waypointLabel->setText(tr("WP") + QString::number(id));
}
}
void UASView::updateThrust(UASInterface* uas, double thrust)
{
if (this->uas == uas)
{
}
}
void UASView::updateBattery(UASInterface* uas, double voltage, double percent, int seconds)
{
Q_UNUSED(voltage);
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
if (this->uas == uas)
{
timeRemaining = seconds;
chargeLevel = percent;
}
}
void UASView::updateState(UASInterface* uas, QString uasState, QString stateDescription)
{
if (this->uas == uas)
{
state = uasState;
stateDesc = stateDescription;
}
}
void UASView::updateLoad(UASInterface* uas, double load)
{
if (this->uas == uas)
{
this->load = load;
}
}
void UASView::refresh()
{
//setUpdatesEnabled(false);
//setUpdatesEnabled(true);
//repaint();
static quint64 lastupdate = 0;
qDebug() << "UASVIEW update diff: " << MG::TIME::getGroundTimeNow() - lastupdate;
lastupdate = MG::TIME::getGroundTimeNow();
// FIXME
static int generalUpdateCount = 0;
if (generalUpdateCount == 4)
{
generalUpdateCount = 0;
qDebug() << "UPDATING EVERYTHING";
// State
m_ui->stateLabel->setText(state);
m_ui->statusTextLabel->setText(stateDesc);
// Battery
m_ui->batteryBar->setValue(static_cast<int>(this->chargeLevel));
//m_ui->loadBar->setValue(static_cast<int>(this->load));
m_ui->thrustBar->setValue(this->thrust);
QString position;
position = position.sprintf("%02.2f %02.2f %02.2f m", x, y, z);
m_ui->positionLabel->setText(position);
QString globalPosition;
QString latIndicator;
if (lat > 0)
{
latIndicator = "N";
}
else
{
latIndicator = "S";
}
QString lonIndicator;
if (lon > 0)
{
lonIndicator = "E";
}
else
{
lonIndicator = "W";
}
globalPosition = globalPosition.sprintf("%02.2f%s %02.2f%s %02.2f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt);
// Altitude
if (groundDistance == 0 && alt != 0)
{
m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt));
}
else
{
m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance));
}
QString speed;
speed = speed.sprintf("%02.2f m/s", totalSpeed);
m_ui->speedLabel->setText(speed);
// Thrust
m_ui->thrustBar->setValue(thrust * 100);
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
if(this->timeRemaining > 1 && this->timeRemaining < MG::MAX_FLIGHT_TIME)
{
// Filter output to get a higher stability
static double filterTime = static_cast<int>(this->timeRemaining);
filterTime = 0.8 * filterTime + 0.2 * static_cast<int>(this->timeRemaining);
int sec = static_cast<int>(filterTime - static_cast<int>(filterTime / 60.0f) * 60);
int min = static_cast<int>(filterTime / 60);
int hours = static_cast<int>(filterTime - min * 60 - sec);
QString timeText;
timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec);
m_ui->timeRemainingLabel->setText(timeText);
}
else
{
m_ui->timeRemainingLabel->setText(tr("Calculating"));
}
// Time Elapsed
//QDateTime time = MG::TIME::msecToQDateTime(uas->getUptime());
quint64 filterTime = uas->getUptime() / 1000;
int sec = static_cast<int>(filterTime - static_cast<int>(filterTime / 60) * 60);
int min = static_cast<int>(filterTime / 60);
int hours = static_cast<int>(filterTime - min * 60 - sec);
QString timeText;
timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec);
m_ui->timeElapsedLabel->setText(timeText);
}
generalUpdateCount++;
// Fade heartbeat icon
// Make color darker
heartbeatColor = heartbeatColor.darker(110);
QString colorstyle;
colorstyle = colorstyle.sprintf("QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X;}",
heartbeatColor.red(), heartbeatColor.green(), heartbeatColor.blue());
m_ui->heartbeatIcon->setStyleSheet(colorstyle);
m_ui->heartbeatIcon->setAutoFillBackground(true);
//setUpdatesEnabled(true);
//setUpdatesEnabled(false);