Commit 3656dc6d authored by Mariano Lizarraga's avatar Mariano Lizarraga

Merge branch 'master' of git://github.com/amolinap/qgroundcontrol into mergeRemote

parents cbc50b8c 2881eaf2
......@@ -256,4 +256,10 @@ void SlugsMAV::emitPidSignal()
}
mavlink_pwm_commands_t* SlugsMAV::getPwmCommands()
{
return &mlPwmCommands;
}
#endif // MAVLINK_ENABLED_SLUGS
......@@ -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:
......
......@@ -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()
......
......@@ -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];
}
......@@ -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();
};
......
......@@ -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/>
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment