From a3c9c11fa63a4ba588c6c744f4d6af50b2069118 Mon Sep 17 00:00:00 2001
From: Mariano Lizarraga <malife@gmail.com>
Date: Tue, 16 Nov 2010 17:16:35 -0600
Subject: [PATCH] First succesful test of HIL Sim. Getting ready to add the
 remaining messages

---
 src/ui/slugshilsim.cc |  87 +++++++++++++++++++++++++++++++++---
 src/ui/slugshilsim.h  |  16 +++++++
 src/ui/slugshilsim.ui | 120 ++++++++++++++++++++++++++++++++++++++++++++++++--
 3 files changed, 214 insertions(+), 9 deletions(-)

diff --git a/src/ui/slugshilsim.cc b/src/ui/slugshilsim.cc
index 08f75b3..0bc179a 100644
--- a/src/ui/slugshilsim.cc
+++ b/src/ui/slugshilsim.cc
@@ -50,6 +50,7 @@ SlugsHilSim::SlugsHilSim(QWidget *parent) :
 
 SlugsHilSim::~SlugsHilSim()
 {
+    rxSocket->disconnectFromHost();
     delete ui;
 }
 
@@ -87,8 +88,9 @@ void SlugsHilSim::putInHilMode(void){
 
     if(msgBox.exec() == QMessageBox::Yes)
     {
+      rxSocket->disconnectFromHost();
       rxSocket->bind(QHostAddress::Any, ui->ed_rxPort->text().toInt());
-      txSocket->bind(QHostAddress::Broadcast, ui->ed_txPort->text().toInt());
+      //txSocket->bind(QHostAddress::Broadcast, ui->ed_txPort->text().toInt());
 
       ui->ed_ipAdress->setEnabled(sw_enableControls);
       ui->ed_rxPort->setEnabled(sw_enableControls);
@@ -108,21 +110,94 @@ void SlugsHilSim::putInHilMode(void){
     ui->cb_mavlinkLinks->setEnabled(sw_enableControls);
 
     ui->bt_startHil->setText(buttonCaption);
+
+    rxSocket->disconnectFromHost();
   }
+}
 
+void SlugsHilSim::readDatagram(void){
+  static int count = 0;
+  while (rxSocket->hasPendingDatagrams()) {
+           QByteArray datagram;
+           datagram.resize(rxSocket->pendingDatagramSize());
+           QHostAddress sender;
+           quint16 senderPort;
+
+           rxSocket->readDatagram(datagram.data(), datagram.size(),
+                                   &sender, &senderPort);
+
+           if (datagram.size() == 113) {
+             processHilDatagram(&datagram);
+           }
+
+           ui->ed_count->setText(QString::number(count++));
+       }
+}
 
 
+void SlugsHilSim::activeUasSet(UASInterface* uas){
 
+  if (uas != NULL) {
+    activeUas = static_cast <UAS *>(uas);
+  }
 }
 
-void SlugsHilSim::readDatagram(void){
 
+void SlugsHilSim::processHilDatagram(const QByteArray* datagram){
+  unsigned char i = 0;
+
+  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++);
+  tmpGpsTime.hour  = datagram->at(i++);
+  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);
+
+  tmpGpsRaw.hdg = getUint16FromDatagram(datagram, &i);
+  tmpGpsRaw.v   = getUint16FromDatagram(datagram, &i);
+  tmpGpsRaw.eph = getUint16FromDatagram(datagram, &i);
+
+  tmpGpsRaw.fix_type = datagram->at(i++);
+  tmpGpsTime.visSat  = datagram->at(i++);
+
+  mavlink_msg_gps_date_time_pack()
+
+  activeUas->sendMessage();
+
+  // 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));
 }
 
+float SlugsHilSim::getFloatFromDatagram (const QByteArray* datagram, unsigned char * i){
+  tFloatToChar tmpF2C;
 
-void SlugsHilSim::activeUasSet(UASInterface* uas){
+  tmpF2C.chData[0] = datagram->at((*i)++);
+  tmpF2C.chData[1] = datagram->at((*i)++);
+  tmpF2C.chData[2] = datagram->at((*i)++);
+  tmpF2C.chData[3] = datagram->at((*i)++);
 
-  if (uas != NULL) {
-    activeUas = uas;
-  }
+  return tmpF2C.flData;
+}
+
+uint16_t SlugsHilSim::getUint16FromDatagram (const QByteArray* datagram, unsigned char * i){
+  tUint16ToChar tmpU2C;
+
+  tmpU2C.chData[0] = datagram->at((*i)++);
+  tmpU2C.chData[1] = datagram->at((*i)++);
+
+  return tmpU2C.uiData;
 }
diff --git a/src/ui/slugshilsim.h b/src/ui/slugshilsim.h
index 98fab34..3255766 100644
--- a/src/ui/slugshilsim.h
+++ b/src/ui/slugshilsim.h
@@ -37,6 +37,7 @@ This file is part of the QGROUNDCONTROL project
 
 #include "LinkInterface.h"
 #include "UAS.h"
+#include <stdint.h>
 
 
 namespace Ui {
@@ -99,9 +100,24 @@ public slots:
 
 
 private:
+
+	typedef union _tFloatToChar {
+		unsigned char   chData[4];
+		float   		flData;
+	} tFloatToChar;
+
+	typedef union _tUint16ToChar {
+		unsigned char   chData[2];
+		uint16_t   		uiData;
+	} 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);
+
 };
 
 #endif // SLUGSHILSIM_H
diff --git a/src/ui/slugshilsim.ui b/src/ui/slugshilsim.ui
index caae1a9..6ed975c 100644
--- a/src/ui/slugshilsim.ui
+++ b/src/ui/slugshilsim.ui
@@ -7,19 +7,19 @@
     <x>0</x>
     <y>0</y>
     <width>325</width>
-    <height>191</height>
+    <height>278</height>
    </rect>
   </property>
   <property name="minimumSize">
    <size>
     <width>320</width>
-    <height>191</height>
+    <height>252</height>
    </size>
   </property>
   <property name="maximumSize">
    <size>
     <width>450</width>
-    <height>229</height>
+    <height>278</height>
    </size>
   </property>
   <property name="font">
@@ -235,6 +235,120 @@
      </item>
     </layout>
    </item>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout_4">
+     <item>
+      <widget class="QLabel" name="label_7">
+       <property name="font">
+        <font>
+         <pointsize>10</pointsize>
+         <weight>75</weight>
+         <bold>true</bold>
+        </font>
+       </property>
+       <property name="layoutDirection">
+        <enum>Qt::LeftToRight</enum>
+       </property>
+       <property name="text">
+        <string>Count</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLineEdit" name="ed_count">
+       <property name="minimumSize">
+        <size>
+         <width>60</width>
+         <height>18</height>
+        </size>
+       </property>
+       <property name="maximumSize">
+        <size>
+         <width>80</width>
+         <height>18</height>
+        </size>
+       </property>
+       <property name="readOnly">
+        <bool>true</bool>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <spacer name="horizontalSpacer_4">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>40</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout_5">
+     <item>
+      <widget class="QLineEdit" name="ed_1">
+       <property name="minimumSize">
+        <size>
+         <width>60</width>
+         <height>18</height>
+        </size>
+       </property>
+       <property name="maximumSize">
+        <size>
+         <width>80</width>
+         <height>18</height>
+        </size>
+       </property>
+       <property name="readOnly">
+        <bool>true</bool>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLineEdit" name="ed_2">
+       <property name="minimumSize">
+        <size>
+         <width>60</width>
+         <height>18</height>
+        </size>
+       </property>
+       <property name="maximumSize">
+        <size>
+         <width>80</width>
+         <height>18</height>
+        </size>
+       </property>
+       <property name="readOnly">
+        <bool>true</bool>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLineEdit" name="ed_3">
+       <property name="minimumSize">
+        <size>
+         <width>60</width>
+         <height>18</height>
+        </size>
+       </property>
+       <property name="maximumSize">
+        <size>
+         <width>80</width>
+         <height>18</height>
+        </size>
+       </property>
+       <property name="readOnly">
+        <bool>true</bool>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
   </layout>
  </widget>
  <resources/>