Skip to content

Commit

Permalink
Merge pull request #4 from ethz-asl/feature/sensorpodStatus
Browse files Browse the repository at this point in the history
Feature/sensorpod status
  • Loading branch information
oberion committed Sep 21, 2015
2 parents 95d9169 + 95f9a3b commit c109238
Show file tree
Hide file tree
Showing 13 changed files with 413 additions and 15 deletions.
2 changes: 1 addition & 1 deletion libs/mavlink/include/mavlink/v1.0
Submodule v1.0 updated from 5a8a44 to 8dfbf2
4 changes: 4 additions & 0 deletions qgroundcontrol.pro
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +153,7 @@ MacBuild {
LinuxBuild {
DEFINES += __STDC_LIMIT_MACROS
CONFIG += qesp_linux_udev
QMAKE_CXXFLAGS += -std=c++11
}

WindowsBuild {
Expand Down Expand Up @@ -353,6 +354,7 @@ FORMS += \
src/ui/QGCCommConfiguration.ui \
src/ui/QGCUDPLinkConfiguration.ui \
src/ui/energybudget.ui \
src/ui/sensorpodstatus.ui \
src/ui/AutoTrim.ui

HEADERS += \
Expand Down Expand Up @@ -508,6 +510,7 @@ HEADERS += \
src/QmlControls/ScreenTools.h \
src/uas/ASLUAV.h \
src/ui/energybudget.h \
src/ui/sensorpodstatus.h \
src/ui/AutoTrim.h

SOURCES += \
Expand Down Expand Up @@ -652,6 +655,7 @@ SOURCES += \
src/ui/toolbar/MainToolBar.cc \
src/QmlControls/ScreenTools.cc \
src/ui/energybudget.cpp \
src/ui/sensorpodstatus.cpp \
src/uas/ASLUAV.cc \
src/ui/AutoTrim.cpp

Expand Down
2 changes: 1 addition & 1 deletion src/comm/MAVLinkSimulationLink.cc
Original file line number Diff line number Diff line change
Expand Up @@ -507,7 +507,7 @@ void MAVLinkSimulationLink::mainloop()
typeCounter++;

// Pack message and get size of encoded byte string
mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_AUTOPILOT_PIXHAWK, system.base_mode, system.custom_mode, system.system_status);
mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_AUTOPILOT_GENERIC, system.base_mode, system.custom_mode, system.system_status);
// Allocate buffer with packet data
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//qDebug() << "CRC:" << msg.ck_a << msg.ck_b;
Expand Down
2 changes: 1 addition & 1 deletion src/comm/MAVLinkSimulationMAV.cc
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ void MAVLinkSimulationMAV::mainloop()
// 1 Hz execution
if (timer1Hz <= 0) {
mavlink_message_t msg;
mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_TYPE_FIXED_WING, MAV_AUTOPILOT_PIXHAWK, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_TYPE_FIXED_WING, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
link->sendMAVLinkMessage(&msg);
planner.handleMessage(msg);

Expand Down
21 changes: 20 additions & 1 deletion src/uas/ASLUAV.cc
Original file line number Diff line number Diff line change
Expand Up @@ -190,6 +190,15 @@ void ASLUAV::receiveMessage(LinkInterface *link, mavlink_message_t message)
//The rest of the message handling (i.e. adding data to the plots) is done elsewhere
break;
}
case MAVLINK_MSG_ID_SENSORPOD_STATUS:
{
mavlink_sensorpod_status_t data;
mavlink_msg_sensorpod_status_decode(&message, &data);
emit SensorpodStatusChanged(data.visensor_rate_1, data.visensor_rate_2, data.visensor_rate_3, data.visensor_rate_4,
data.recording_nodes_count,data.cpu_temp, data.free_space);

break;
}
default:
{
//std::cout << "msgid:"<<message.(int)msgid<<endl; //"ASLUAV: Unknown message received"<<endl;
Expand Down Expand Up @@ -258,4 +267,14 @@ int ASLUAV::SendCommandLong(MAV_CMD CmdID, float param1, float param2, float par

std::cout << "ASLUAV: Command with ID #"<<CmdID<<" sent." << std::endl;
return 0;
}
}

int ASLUAV::SendCommandLongTarget(MAV_CMD CmdID, uint8_t target_component, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
{
mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, target_component, CmdID, 1, param1, param2, param3, param4, param5, param6, param7);
sendMessage(msg);

std::cout << "ASLUAV: Command with ID #"<<CmdID<<" sent to component " << (int) target_component << "." << std::endl;
return 0;
}
3 changes: 2 additions & 1 deletion src/uas/ASLUAV.h
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ class ASLUAV : public UAS
public:
QString getAutopilotTypeName(); //Overwritten from UAS to add MAV_AUTOPILOT_ASLUAV type.
int SendCommandLong(MAV_CMD CmdID, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
int SendCommandLongTarget(MAV_CMD CmdID, uint8_t target_component, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);

public slots:
/** @brief Receive a MAVLink message from this MAV */
Expand All @@ -31,7 +32,7 @@ public slots:
void PowerDataChanged(float volt, float currpb, float curr_1, float curr_2);
void MPPTDataChanged(float volt1, float amp1, uint16_t pwm1, uint8_t status1, float volt2, float amp2, uint16_t pwm2, uint8_t status2, float volt3, float amp3, uint16_t pwm3, uint8_t status3);
void BatMonDataChanged(uint8_t compid, uint16_t volt, int16_t current, uint8_t soc, float temp, uint16_t batStatus, uint16_t hostfetcontrol, uint16_t cellvolt1, uint16_t cellvolt2, uint16_t cellvolt3, uint16_t cellvolt4, uint16_t cellvolt5, uint16_t cellvolt6);

void SensorpodStatusChanged(uint8_t rate1, uint8_t rate2, uint8_t rate3, uint8_t rate4, uint8_t numRecordTopics, uint8_t cpuTemp, uint16_t freeSpace);

protected:
// Add an external voltage sensor in addition to PX4-onboard sensor
Expand Down
6 changes: 3 additions & 3 deletions src/uas/UAS.h
Original file line number Diff line number Diff line change
Expand Up @@ -615,9 +615,9 @@ class UAS : public UASInterface
case MAV_AUTOPILOT_GENERIC:
return "GENERIC";
break;
case MAV_AUTOPILOT_PIXHAWK:
return "PIXHAWK";
break;
// case MAV_AUTOPILOT_PIXHAWK:
// return "PIXHAWK";
// break;
case MAV_AUTOPILOT_SLUGS:
return "SLUGS";
break;
Expand Down
9 changes: 7 additions & 2 deletions src/ui/MainWindow.cc
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,7 @@ This file is part of the QGROUNDCONTROL project
#include "QGCMessageBox.h"
#include "QGCDockWidget.h"
#include "energybudget.h"
#include "sensorpodstatus.h"

#ifdef QGC_USE_ASLUAV_MESSAGES
#include "AutoTrim.h"
Expand Down Expand Up @@ -104,6 +105,7 @@ const char* MainWindow::_uasInfoViewDockWidgetName = "UAS_INFO_INFOVIEW_DOCKWIDG
const char* MainWindow::_debugConsoleDockWidgetName = "COMMUNICATION_CONSOLE_DOCKWIDGET";
const char* MainWindow::_AutoTrimDockWidgetName = "AUTOTRIM_DOCKWIDGET";
const char* MainWindow::_aslEnergyBudgetWidgetName = "ENERGY_BUDGET_DOCKWIDGET";
const char* MainWindow::_aslSensorpodStatusWidgetName = "SENSORPOD_STATUS_DOCKWIDGET";

static MainWindow* _instance = NULL; ///< @brief MainWindow singleton

Expand Down Expand Up @@ -431,8 +433,9 @@ void MainWindow::_buildCommonWidgets(void)
{ _uasInfoViewDockWidgetName, "Info View", Qt::LeftDockWidgetArea },
{ _debugConsoleDockWidgetName, "Communications Console", Qt::LeftDockWidgetArea },
{ _AutoTrimDockWidgetName, "Auto Trim", Qt::RightDockWidgetArea },
{ _aslEnergyBudgetWidgetName, "Energy Budget", Qt::RightDockWidgetArea }

{ _aslEnergyBudgetWidgetName, "Energy Budget", Qt::RightDockWidgetArea },
{ _aslSensorpodStatusWidgetName, "Sensorpod Status", Qt::RightDockWidgetArea }

};
static const size_t cDockWidgetInfo = sizeof(rgDockWidgetInfo) / sizeof(rgDockWidgetInfo[0]);

Expand Down Expand Up @@ -585,6 +588,8 @@ void MainWindow::_createInnerDockWidget(const QString& widgetName)
widget = new QGCTabbedInfoView(this);
} else if (widgetName == _aslEnergyBudgetWidgetName) {
widget = new EnergyBudget(this);
} else if (widgetName == _aslSensorpodStatusWidgetName) {
widget = new SensorpodStatus(this);
} else if (widgetName == _debugConsoleDockWidgetName) {
widget = new DebugConsole(this);
} else if (widgetName == _AutoTrimDockWidgetName) {
Expand Down
1 change: 1 addition & 0 deletions src/ui/MainWindow.h
Original file line number Diff line number Diff line change
Expand Up @@ -335,6 +335,7 @@ private slots:
static const char* _debugConsoleDockWidgetName;
static const char* _AutoTrimDockWidgetName;
static const char* _aslEnergyBudgetWidgetName;
static const char* _aslSensorpodStatusWidgetName;

QMap<QString, QDockWidget*> _mapName2DockWidget;
QMap<int, QDockWidget*> _mapUasId2HilDockWidget;
Expand Down
14 changes: 9 additions & 5 deletions src/ui/energybudget.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -65,8 +65,8 @@ m_mpptHyst(new Hysteresisf(CELLPOWERHYSTMIN, CELLPOWERHYSTMAX, true)),
m_MPPTUpdateReset(new QTimer(this))
{
ui->setupUi(this);
ui->overviewGraphicsView->setHorizontalScrollBarPolicy(Qt::ScrollBarPolicy::ScrollBarAlwaysOff);
ui->overviewGraphicsView->setVerticalScrollBarPolicy(Qt::ScrollBarPolicy::ScrollBarAlwaysOff);
ui->overviewGraphicsView->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
ui->overviewGraphicsView->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
this->buildGraphicsImage();
ui->overviewGraphicsView->setScene(m_scene);
ui->overviewGraphicsView->fitInView(m_scene->sceneRect(), Qt::KeepAspectRatio);
Expand Down Expand Up @@ -99,9 +99,13 @@ void EnergyBudget::buildGraphicsImage()
m_scene->setSceneRect(0.0, 0.0, 1000.0, 1000.0);
qreal penWidth(40);
// scale and place images
m_propPixmap->setScale(this->adjustImageScale(m_scene->sceneRect(), m_propPixmap->boundingRect()));
m_cellPixmap->setScale(this->adjustImageScale(m_scene->sceneRect(), m_cellPixmap->boundingRect()));
m_batPixmap->setScale(this->adjustImageScale(m_scene->sceneRect(), m_batPixmap->boundingRect()));

QRectF tempRectF=m_propPixmap->boundingRect();
m_propPixmap->setScale(this->adjustImageScale(m_scene->sceneRect(), tempRectF ));
tempRectF=m_cellPixmap->boundingRect();
m_cellPixmap->setScale(this->adjustImageScale(m_scene->sceneRect(), tempRectF ));
tempRectF=m_batPixmap->boundingRect();
m_batPixmap->setScale(this->adjustImageScale(m_scene->sceneRect(), tempRectF ));
double maxheight(std::fmax(m_propPixmap->mapRectToScene(m_propPixmap->boundingRect()).height(), m_batPixmap->mapRectToScene(m_batPixmap->boundingRect()).height()));
m_propPixmap->setOffset(0.0, m_propPixmap->mapRectFromScene(m_scene->sceneRect()).height() - m_propPixmap->mapRectFromScene(QRectF(0.0,0.0,1.0,maxheight)).height()/2.0 - m_propPixmap->boundingRect().height() / 2.0);
m_batPixmap->setOffset(m_batPixmap->mapRectFromScene(m_scene->sceneRect()).width() - m_batPixmap->boundingRect().width(), m_batPixmap->mapRectFromScene(m_scene->sceneRect()).height() - m_batPixmap->mapRectFromScene(QRectF(0.0, 0.0, 1.0, maxheight)).height() / 2.0 - m_batPixmap->boundingRect().height() / 2.0);
Expand Down
102 changes: 102 additions & 0 deletions src/ui/sensorpodstatus.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,102 @@
#include "sensorpodstatus.h"
#include "ui_sensorpodstatus.h"
#include <qgraphicsscene.h>
#include <QMessageBox>
#include <QTimer>
#include <math.h>
#include <qdebug.h>
#include "../uas/ASLUAV.h"
#include "UASInterface.h"
#include "UASManager.h"
#include "QGCApplication.h"

#define RESETTIMEMS 10000

SensorpodStatus::SensorpodStatus(QWidget *parent) :
QWidget(parent),
ui(new Ui::SensorpodStatus),
m_scene(new QGraphicsScene(this)),
m_UpdateReset(new QTimer (this))
{
ui->setupUi(this);
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
connect(ui->PowerCycleButton, SIGNAL(clicked()), this, SLOT(PowerCycleSensorpodCmd()));
// connect(qgcApp(), SIGNAL(styleChanged(bool)), this, SLOT(styleChanged(bool)));
m_UpdateReset->setInterval(RESETTIMEMS);
m_UpdateReset->setSingleShot(false);
connect(m_UpdateReset, SIGNAL(timeout()), this, SLOT(UpdateTimerTimeout()));
if (UASManager::instance()->getActiveUAS())
{
setActiveUAS(UASManager::instance()->getActiveUAS());
}
m_UpdateReset->start();
}

SensorpodStatus::~SensorpodStatus()
{
delete ui;
delete m_scene;
delete m_UpdateReset;
}

void SensorpodStatus::updateSensorpodStatus(uint8_t rate1, uint8_t rate2, uint8_t rate3, uint8_t rate4, uint8_t numRecordTopics, uint8_t cpuTemp, uint16_t freeSpace)
{
m_UpdateReset->stop();
ui->topic1rate->setText(QString("%1 Hz").arg(rate1));
ui->topic2rate->setText(QString("%1 Hz").arg(rate2));
ui->topic3rate->setText(QString("%1 Hz").arg(rate3));
ui->topic4rate->setText(QString("%1 Hz").arg(rate4));
ui->numRecNodes->setText(QString("%1").arg(numRecordTopics));
ui->cpuTemp->setText(QString("%1 °C").arg(cpuTemp));
ui->freeDiskSpace->setText(QString("%1 GB").arg(freeSpace/100.0));
m_UpdateReset->start(RESETTIMEMS);
}


//void EnergyBudget::resizeEvent(QResizeEvent *event)
//{
// QWidget::resizeEvent(event);
// ui->overviewGraphicsView->fitInView(m_scene->sceneRect(), Qt::AspectRatioMode::KeepAspectRatio);
//}

void SensorpodStatus::setActiveUAS(UASInterface *uas)
{
//disconnect any previous uas
disconnect(this, SLOT(updateSensorpodStatus(uint8_t, uint8_t, uint8_t, uint8_t, uint8_t, uint8_t, uint16_t)));

//connect the uas if asluas
ASLUAV *asluas = dynamic_cast<ASLUAV*>(uas);
if (asluas)
{
connect(asluas, SIGNAL(SensorpodStatusChanged(uint8_t, uint8_t, uint8_t, uint8_t, uint8_t, uint8_t, uint16_t)), this, SLOT(updateSensorpodStatus(uint8_t, uint8_t, uint8_t, uint8_t, uint8_t, uint8_t, uint16_t)));
}
//else set to standard output
else
{

}
}

void SensorpodStatus::PowerCycleSensorpodCmd()
{
QMessageBox::StandardButton reply;
reply = QMessageBox::question(this, tr("Sensorpod reset"), tr("Sending command to perform power cycle of sensorpod. Use this with caution! Are you sure?"), QMessageBox::Yes | QMessageBox::No);

if (reply == QMessageBox::Yes) {
//Send the message via the currently active UAS
ASLUAV *tempUAS = (ASLUAV*) UASManager::instance()->getActiveUAS();;
if (tempUAS) tempUAS->SendCommandLongTarget(MAV_CMD_PAYLOAD_CONTROL, (uint8_t) 50, 1.0f);
}

}

void SensorpodStatus::UpdateTimerTimeout()
{
ui->topic1rate->setText(QString("--"));
ui->topic2rate->setText(QString("--"));
ui->topic3rate->setText(QString("--"));
ui->topic4rate->setText(QString("--"));
ui->numRecNodes->setText(QString("--"));
ui->cpuTemp->setText(QString("--"));
ui->freeDiskSpace->setText(QString("--"));
}
39 changes: 39 additions & 0 deletions src/ui/sensorpodstatus.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
#ifndef SENSORPODSTATUS_H
#define SENSORPODSTATUS_H

#include <QObject>
#include <QWidget>
#include <stdint.h>

namespace Ui {
class SensorpodStatus;
}

class QGraphicsScene;
class UASInterface;
class QTimer;
class UASInterface;

class SensorpodStatus : public QWidget
{
Q_OBJECT

public:
explicit SensorpodStatus(QWidget *parent = 0);
~SensorpodStatus();

protected:
Ui::SensorpodStatus *ui;
QGraphicsScene *m_scene;
QTimer *m_UpdateReset;


protected slots:
void updateSensorpodStatus(uint8_t rate1, uint8_t rate2, uint8_t rate3, uint8_t rate4, uint8_t numRecordTopics, uint8_t cpuTemp, uint16_t freeSpace);
void setActiveUAS(UASInterface *uas);
void UpdateTimerTimeout();

void PowerCycleSensorpodCmd();
};

#endif // SENSORPODSTATUS_H
Loading

0 comments on commit c109238

Please sign in to comment.