send and receive datagram

src/uas/SlugsMAV.cc

@@ -255,4 +255,10 @@ void SlugsMAV::emitPidSignal()
 
 }
 
+
+mavlink_pwm_commands_t* SlugsMAV::getPwmCommands()
+{
+    return &mlPwmCommands;
+}
+
 #endif // MAVLINK_ENABLED_SLUGS

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:

src/ui/MainWindow.cc

@@ -201,6 +201,22 @@ void MainWindow::buildCommonWidgets()
         protocolWidget    = new XMLCommProtocolWidget(this);
         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);
+    }
 }
 
 void MainWindow::buildPxWidgets()
@@ -378,12 +394,12 @@ void MainWindow::buildSlugsWidgets()
         addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget()), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea);
     }
 
-    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 (!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);
+//    }
 }
 
 
@@ -702,6 +718,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()

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,13 @@ 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));
 }
 
 SlugsHilSim::~SlugsHilSim()
@@ -121,6 +132,10 @@ void SlugsHilSim::readDatagram(void){
 
            if (datagram.size() == 113) {
              processHilDatagram(&datagram);
+
+             sendMessageToSlugs();
+
+             commandToSimulink();
            }
 
            ui->ed_count->setText(QString::number(count++));
@@ -143,11 +158,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 +165,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);
+
+  tmpGpsData.hdg = getUint16FromDatagram(datagram, &i);
+  tmpGpsData.v   = getUint16FromDatagram(datagram, &i);
+
+  tmpGpsData.eph = getUint16FromDatagram(datagram, &i);
+  tmpGpsData.fix_type = datagram->at(i++);
+  tmpGpsTime.visSat = datagram->at(i++);
+  i++;
+
+  tmpAirData.dynamicPressure= getFloatFromDatagram(datagram, &i);
+  tmpAirData.staticPressure= getFloatFromDatagram(datagram, &i);
+  tmpAirData.temperature= getUint16FromDatagram(datagram, &i);
+
+  // TODO Salto en el Datagrama
+  i=i+8;
+
+  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);
 
-  tmpGpsRaw.hdg = getUint16FromDatagram(datagram, &i);
-  tmpGpsRaw.v   = getUint16FromDatagram(datagram, &i);
-  tmpGpsRaw.eph = getUint16FromDatagram(datagram, &i);
+  // TODO Crear Paquete SYNC TIME
+  i=i+2;
 
-  tmpGpsRaw.fix_type = datagram->at(i++);
-  tmpGpsTime.visSat  = datagram->at(i++);
+  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_date_time_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsTime);
+//  activeUas->sendMessage(hilLink, msg);
 
-  mavlink_msg_gps_raw_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsRaw);
-  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
@@ -210,3 +262,84 @@ void SlugsHilSim::linkSelected(int cbIndex){
   //hilLink = linksAvailable
     // FIXME Mariano
 }
+
+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::commandToSimulink()
+{
+    //mavlink_pwm_commands_t* pwdC = (static_cast<SlugsMAV*>(activeUas))->getPwmCommands();
+
+    uint16_t j=10;
+    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);
+
+    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];
+}

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 commandToSimulink();
 
 };
 

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/>