diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc
index 176558c172bb2c6d1c20fb04c925fb37b6ec981e..6126ab0a88df410225c934f01e7f1dd8bb7e9d1e 100644
--- a/src/uas/SlugsMAV.cc
+++ b/src/uas/SlugsMAV.cc
@@ -256,4 +256,10 @@ void SlugsMAV::emitPidSignal()
}
+
+mavlink_pwm_commands_t* SlugsMAV::getPwmCommands()
+{
+ return &mlPwmCommands;
+}
+
#endif // MAVLINK_ENABLED_SLUGS
diff --git a/src/uas/SlugsMAV.h b/src/uas/SlugsMAV.h
index c30996d32369dbb66290b71af97408fac7cd748b..ce0d0a3d10048d226af62f6d33488d4fd11c75b5 100644
--- a/src/uas/SlugsMAV.h
+++ b/src/uas/SlugsMAV.h
@@ -36,12 +36,15 @@ class SlugsMAV : public UAS
public:
SlugsMAV(MAVLinkProtocol* mavlink, int id = 0);
+
+
public slots:
/** @brief Receive a MAVLink message from this MAV */
void receiveMessage(LinkInterface* link, mavlink_message_t message);
void emitSignals (void);
+ mavlink_pwm_commands_t* getPwmCommands();
signals:
@@ -70,6 +73,8 @@ signals:
+
+
#endif
protected:
diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc
index 134d68fd1768c4ae9ff320f727f9212a73c37be5..1533afba5a5ac7de04076caabaf419686b72d43b 100644
--- a/src/ui/MainWindow.cc
+++ b/src/ui/MainWindow.cc
@@ -292,6 +292,21 @@ void MainWindow::buildCommonWidgets()
addToCentralWidgetsMenu (protocolWidget, "Mavlink Generator", SLOT(showCentralWidget()),CENTRAL_PROTOCOL);
}
+ //TODO temporaly debug
+ if (!slugsHilSimWidget)
+ {
+ slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this);
+ slugsHilSimWidget->setWidget( new SlugsHilSim(this));
+ addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget()), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea);
+ }
+
+ //TODO temporaly debug
+ if (!slugsCamControlWidget)
+ {
+ slugsCamControlWidget = new QDockWidget(tr("Slugs Video Camera Control"), this);
+ slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this));
+ addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget()), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea);
+ }
if (!dataplotWidget)
{
dataplotWidget = new QGCDataPlot2D(this);
@@ -798,6 +813,12 @@ void MainWindow::connectCommonWidgets()
// it notifies that a waypoint global goes to do create and a map graphic too
connect(waypointsDockWidget->widget(), SIGNAL(createWaypointAtMap(QPointF)), mapWidget, SLOT(createWaypointGraphAtMap(QPointF)));
}
+
+ //TODO temporaly debug
+ if (slugsHilSimWidget && slugsHilSimWidget->widget()){
+ connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
+ slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*)));
+ }
}
void MainWindow::createCustomWidget()
diff --git a/src/ui/SlugsHilSim.cc b/src/ui/SlugsHilSim.cc
index 029284a47ecc28d65c3e67cc0d3e56fc20869374..bd06c8b3ca2ea348862178f9417004e427d46082 100644
--- a/src/ui/SlugsHilSim.cc
+++ b/src/ui/SlugsHilSim.cc
@@ -32,6 +32,10 @@ This file is part of the QGROUNDCONTROL project
#include "ui_SlugsHilSim.h"
#include "LinkManager.h"
+#include "SlugsMAV.h"
+#include "qbytearray.h"
+#include "qhostaddress.h"
+
SlugsHilSim::SlugsHilSim(QWidget *parent) :
QWidget(parent),
ui(new Ui::SlugsHilSim)
@@ -49,6 +53,18 @@ SlugsHilSim::SlugsHilSim(QWidget *parent) :
connect(rxSocket, SIGNAL(readyRead()), this, SLOT(readDatagram()));
linksAvailable.clear();
+
+ memset(&tmpAirData, 0, sizeof(mavlink_air_data_t));
+ memset(&tmpAttitudeData, 0, sizeof(mavlink_attitude_t));
+ memset(&tmpGpsData, 0, sizeof(mavlink_gps_raw_t));
+ memset(&tmpGpsTime, 0, sizeof(mavlink_gps_date_time_t));
+ memset(&tmpLocalPositionData, 0, sizeof(mavlink_sensor_bias_t));
+ memset(&tmpRawImuData, 0, sizeof(mavlink_raw_imu_t));
+
+ foreach (LinkInterface* link, LinkManager::instance()->getLinks())
+ {
+ addToCombo(link);
+ }
}
SlugsHilSim::~SlugsHilSim()
@@ -121,6 +137,10 @@ void SlugsHilSim::readDatagram(void){
if (datagram.size() == 113) {
processHilDatagram(&datagram);
+
+ sendMessageToSlugs();
+
+ commandDatagramToSimulink();
}
ui->ed_count->setText(QString::number(count++));
@@ -143,11 +163,6 @@ void SlugsHilSim::processHilDatagram(const QByteArray* datagram)
mavlink_message_t msg;
- // GPS
- mavlink_gps_raw_t tmpGpsRaw;
-
- mavlink_gps_date_time_t tmpGpsTime;
-
tmpGpsTime.year = datagram->at(i++);
tmpGpsTime.month = datagram->at(i++);
tmpGpsTime.day = datagram->at(i++);
@@ -155,31 +170,73 @@ void SlugsHilSim::processHilDatagram(const QByteArray* datagram)
tmpGpsTime.min = datagram->at(i++);
tmpGpsTime.sec = datagram->at(i++);
- tmpGpsRaw.lat = getFloatFromDatagram(datagram, &i);
- tmpGpsRaw.lon = getFloatFromDatagram(datagram, &i);
- tmpGpsRaw.alt = getFloatFromDatagram(datagram, &i);
+ tmpGpsData.lat = getFloatFromDatagram(datagram, &i);
+ tmpGpsData.lon = getFloatFromDatagram(datagram, &i);
+ tmpGpsData.alt = getFloatFromDatagram(datagram, &i);
- tmpGpsRaw.hdg = getUint16FromDatagram(datagram, &i);
- tmpGpsRaw.v = getUint16FromDatagram(datagram, &i);
- tmpGpsRaw.eph = getUint16FromDatagram(datagram, &i);
+ tmpGpsData.hdg = getUint16FromDatagram(datagram, &i);
+ tmpGpsData.v = getUint16FromDatagram(datagram, &i);
- tmpGpsRaw.fix_type = datagram->at(i++);
+ tmpGpsData.eph = getUint16FromDatagram(datagram, &i);
+ tmpGpsData.fix_type = datagram->at(i++);
tmpGpsTime.visSat = datagram->at(i++);
+ i++;
- mavlink_msg_gps_date_time_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsTime);
- activeUas->sendMessage(hilLink, msg);
+ tmpAirData.dynamicPressure= getFloatFromDatagram(datagram, &i);
+ tmpAirData.staticPressure= getFloatFromDatagram(datagram, &i);
+ tmpAirData.temperature= getUint16FromDatagram(datagram, &i);
- memset(&msg, 0, sizeof(mavlink_message_t));
+ // TODO Salto en el Datagrama
+ i=i+8;
- mavlink_msg_gps_raw_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsRaw);
- activeUas->sendMessage(hilLink,msg);
+ tmpRawImuData.xgyro = getUint16FromDatagram(datagram, &i);
+ tmpRawImuData.ygyro = getUint16FromDatagram(datagram, &i);
+ tmpRawImuData.zgyro = getUint16FromDatagram(datagram, &i);
+ tmpRawImuData.xacc = getUint16FromDatagram(datagram, &i);
+ tmpRawImuData.yacc = getUint16FromDatagram(datagram, &i);
+ tmpRawImuData.zacc = getUint16FromDatagram(datagram, &i);
+ tmpRawImuData.xmag = getUint16FromDatagram(datagram, &i);
+ tmpRawImuData.ymag = getUint16FromDatagram(datagram, &i);
+ tmpRawImuData.zmag = getUint16FromDatagram(datagram, &i);
+
+ tmpAttitudeData.roll = getFloatFromDatagram(datagram, &i);
+ tmpAttitudeData.pitch = getFloatFromDatagram(datagram, &i);
+ tmpAttitudeData.yaw = getFloatFromDatagram(datagram, &i);
+
+ tmpAttitudeData.rollspeed = getFloatFromDatagram(datagram, &i);
+ tmpAttitudeData.pitchspeed = getFloatFromDatagram(datagram, &i);
+ tmpAttitudeData.yawspeed = getFloatFromDatagram(datagram, &i);
+
+ // TODO Crear Paquete SYNC TIME
+ i=i+2;
+
+ tmpLocalPositionData.x = getFloatFromDatagram(datagram, &i);
+ tmpLocalPositionData.y = getFloatFromDatagram(datagram, &i);
+ tmpLocalPositionData.z = getFloatFromDatagram(datagram, &i);
+ tmpLocalPositionData.vx = getFloatFromDatagram(datagram, &i);
+ tmpLocalPositionData.vy = getFloatFromDatagram(datagram, &i);
+ tmpLocalPositionData.vz = getFloatFromDatagram(datagram, &i);
+
+
+// mavlink_msg_gps_date_time_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsTime);
+// activeUas->sendMessage(hilLink, msg);
+
+// memset(&msg, 0, sizeof(mavlink_message_t));
+
+// mavlink_msg_gps_raw_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsRaw);
+// activeUas->sendMessage(hilLink,msg);
// TODO: this is legacy of old HIL datagram. Need to remove from Simulink model
i++;
- ui->ed_1->setText(QString::number(tmpGpsRaw.hdg));
- ui->ed_2->setText(QString::number(tmpGpsRaw.v));
- ui->ed_3->setText(QString::number(tmpGpsRaw.eph));
+ ui->ed_1->setText(QString::number(tmpRawImuData.xacc));
+ ui->ed_2->setText(QString::number(tmpRawImuData.yacc));
+ ui->ed_3->setText(QString::number(tmpRawImuData.zacc));
+
+ ui->tbA->setText(QString::number(tmpRawImuData.xgyro));
+ ui->tbB->setText(QString::number(tmpRawImuData.ygyro));
+ ui->tbC->setText(QString::number(tmpRawImuData.zgyro));
+
#else
Q_UNUSED(datagram);
#endif
@@ -209,9 +266,93 @@ uint16_t SlugsHilSim::getUint16FromDatagram (const QByteArray* datagram, unsigne
void SlugsHilSim::linkSelected(int cbIndex){
#ifdef MAVLINK_ENABLED_SLUGS
// HIL code to go here...
- //hilLink = linksAvailable
- Q_UNUSED(cbIndex);
- #else
- Q_UNUSED(cbIndex);
- #endif
+ //hilLink = linksAvailable
+ // FIXME Mariano
+
+ hilLink = LinkManager::instance()->getLinkForId(cbIndex);
+}
+
+void SlugsHilSim::sendMessageToSlugs()
+{
+ mavlink_message_t msg;
+
+ mavlink_msg_local_position_encode(MG::SYSTEM::ID,
+ MG::SYSTEM::COMPID,
+ &msg,
+ &tmpLocalPositionData);
+ activeUas->sendMessage(hilLink, msg);
+ memset(&msg, 0, sizeof(mavlink_message_t));
+
+ mavlink_msg_attitude_encode(MG::SYSTEM::ID,
+ MG::SYSTEM::COMPID,
+ &msg,
+ &tmpAttitudeData);
+ activeUas->sendMessage(hilLink, msg);
+ memset(&msg, 0, sizeof(mavlink_message_t));
+
+ mavlink_msg_raw_imu_encode(MG::SYSTEM::ID,
+ MG::SYSTEM::COMPID,
+ &msg,
+ &tmpRawImuData);
+ activeUas->sendMessage(hilLink, msg);
+ memset(&msg, 0, sizeof(mavlink_message_t));
+
+ mavlink_msg_air_data_encode(MG::SYSTEM::ID,
+ MG::SYSTEM::COMPID,
+ &msg,
+ &tmpAirData);
+ activeUas->sendMessage(hilLink, msg);
+ memset(&msg, 0, sizeof(mavlink_message_t));
+
+ mavlink_msg_gps_raw_encode(MG::SYSTEM::ID,
+ MG::SYSTEM::COMPID,
+ &msg,
+ &tmpGpsData);
+ activeUas->sendMessage(hilLink, msg);
+ memset(&msg, 0, sizeof(mavlink_message_t));
+
+ mavlink_msg_gps_date_time_encode(MG::SYSTEM::ID,
+ MG::SYSTEM::COMPID,
+ &msg,
+ &tmpGpsTime);
+ activeUas->sendMessage(hilLink, msg);
+ memset(&msg, 0, sizeof(mavlink_message_t));
+}
+
+
+void SlugsHilSim::commandDatagramToSimulink()
+{
+ //mavlink_pwm_commands_t* pwdC = (static_cast<SlugsMAV*>(activeUas))->getPwmCommands();
+
+ mavlink_pwm_commands_t* pwdC;
+
+ if(pwdC != NULL){
+ }
+
+ QByteArray data;
+ data.resize(22);
+
+ unsigned char i=0;
+ setUInt16ToDatagram(data, &i, 1);//pwdC->dt_c);
+ setUInt16ToDatagram(data, &i, 2);//pwdC->dla_c);
+ setUInt16ToDatagram(data, &i, 3);//pwdC->dra_c);
+ setUInt16ToDatagram(data, &i, 4);//pwdC->dr_c);
+ setUInt16ToDatagram(data, &i, 5);//pwdC->dle_c);
+ setUInt16ToDatagram(data, &i, 6);//pwdC->dre_c);
+ setUInt16ToDatagram(data, &i, 7);//pwdC->dlf_c);
+ setUInt16ToDatagram(data, &i, 8);//pwdC->drf_c);
+ setUInt16ToDatagram(data, &i, 9);//pwdC->aux1);
+ setUInt16ToDatagram(data, &i, 10);//pwdC->aux2);
+ setUInt16ToDatagram(data, &i, 11);//value default
+
+ txSocket->writeDatagram(data, QHostAddress::Broadcast, ui->ed_txPort->text().toInt());
+}
+
+void SlugsHilSim::setUInt16ToDatagram(QByteArray& datagram, unsigned char* pos, uint16_t value)
+{
+ tUint16ToChar tmpUnion;
+ tmpUnion.uiData= value;
+
+ datagram[(*pos)++]= tmpUnion.chData[0];
+ datagram[(*pos)++]= tmpUnion.chData[1];
}
diff --git a/src/ui/SlugsHilSim.h b/src/ui/SlugsHilSim.h
index dc0d8d1d7a90909656c29145de053c7231470cb3..11326ee9ae68c668a03a844b15842c17c5fc8c15 100644
--- a/src/ui/SlugsHilSim.h
+++ b/src/ui/SlugsHilSim.h
@@ -52,6 +52,8 @@ public:
explicit SlugsHilSim(QWidget *parent = 0);
~SlugsHilSim();
+
+
protected:
LinkInterface* hilLink;
QHostAddress* simulinkIp;
@@ -59,6 +61,13 @@ protected:
QUdpSocket* rxSocket;
UAS* activeUas;
+ mavlink_local_position_t tmpLocalPositionData;
+ mavlink_attitude_t tmpAttitudeData;
+ mavlink_raw_imu_t tmpRawImuData;
+ mavlink_air_data_t tmpAirData;
+ mavlink_gps_raw_t tmpGpsData;
+ mavlink_gps_date_time_t tmpGpsTime;
+
public slots:
/**
@@ -121,11 +130,18 @@ private:
} tUint16ToChar;
Ui::SlugsHilSim *ui;
+
QHash <int, LinkInterface*> linksAvailable;
void processHilDatagram (const QByteArray* datagram);
float getFloatFromDatagram (const QByteArray* datagram, unsigned char * i);
uint16_t getUint16FromDatagram (const QByteArray* datagram, unsigned char * i);
+ void setUInt16ToDatagram(QByteArray& datagram, unsigned char* pos, uint16_t value);
+
+
+ void sendMessageToSlugs();
+
+ void commandDatagramToSimulink();
};
diff --git a/src/ui/SlugsHilSim.ui b/src/ui/SlugsHilSim.ui
index 6ed975ca47e28fd913cb87f1326346dac1ba5d51..7edcb1ea7f22c4b8a1c2443c6145572307063668 100644
--- a/src/ui/SlugsHilSim.ui
+++ b/src/ui/SlugsHilSim.ui
@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>325</width>
+ <width>337</width>
<height>278</height>
</rect>
</property>
@@ -349,6 +349,19 @@
</item>
</layout>
</item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_6">
+ <item>
+ <widget class="QLineEdit" name="tbA"/>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="tbB"/>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="tbC"/>
+ </item>
+ </layout>
+ </item>
</layout>
</widget>
<resources/>