Continuing work in HIL Sim for Slugs

src/comm/SerialLink.cc

@@ -64,7 +64,8 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P
     // Set the port name
     if (porthandle == "")
     {
-        name = tr("serial link ") + QString::number(getId()) + tr(" (unconfigured)");
+//        name = tr("serial link ") + QString::number(getId()) + tr(" (unconfigured)");
+        name = tr("Serial Link ") + QString::number(getId());
     }
     else
     {

src/ui/MainWindow.cc

@@ -181,6 +181,8 @@ void MainWindow::buildWidgets()
     slugsDataWidget = new QDockWidget(tr("Slugs Data"), this);
     slugsDataWidget->setWidget( new SlugsDataSensorView(this));
 
+    slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this);
+    slugsHilSimWidget->setWidget( new SlugsHilSim(this));
 
 }
 
@@ -215,9 +217,14 @@ void MainWindow::connectWidgets()
 
         // 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)));
-        // it notifies that a waypoint global change it´s position by spinBox on Widget WaypointView
+        // it notifies that a waypoint global change it¥s position by spinBox on Widget WaypointView
         connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float)));
     }
+
+    if (slugsHilSimWidget->widget()){
+      connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), dynamic_cast<SlugsHilSim*>(slugsHilSimWidget->widget()), SLOT(activeUasSet(UASInterface*)) );
+    }
+
 }
 
 void MainWindow::arrangeCenterStack()
@@ -540,15 +547,13 @@ void MainWindow::UASCreated(UASInterface* uas)
         // Check which type this UAS is of
         PxQuadMAV* mav = dynamic_cast<PxQuadMAV*>(uas);
         if (mav) loadPixhawkView();
+
         SlugsMAV* mav2 = dynamic_cast<SlugsMAV*>(uas);
         if (mav2)
         {
-            SlugsDataSensorView* slugDataView = dynamic_cast<SlugsDataSensorView*>(slugsDataWidget->widget());
+          dynamic_cast<SlugsDataSensorView*>(slugsDataWidget->widget())->addUAS(uas);
-            if(slugDataView)
-            {
-                slugDataView->addUAS(uas);
-            }
           loadSlugsView();
+
         }
 
 
@@ -639,17 +644,6 @@ void MainWindow::loadSlugsView()
         infoDockWidget->show();
     }
 
-    // HORIZONTAL SITUATION INDICATOR
-    if (hsiDockWidget)
-    {
-        HSIDisplay* hsi = dynamic_cast<HSIDisplay*>( hsiDockWidget->widget() );
-        if (hsi)
-        {
-            hsi->start();
-            addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget);
-            hsiDockWidget->show();
-        }
-    }
 
     // WAYPOINT LIST
     if (waypointsDockWidget)
@@ -672,6 +666,12 @@ void MainWindow::loadSlugsView()
         slugsDataWidget->show();
     }
 
+    // Slugs Data View
+    if (slugsHilSimWidget)
+    {
+        addDockWidget(Qt::LeftDockWidgetArea, slugsHilSimWidget);
+        slugsHilSimWidget->show();
+    }
     this->show();
 }
 

src/ui/MainWindow.h

@@ -66,6 +66,7 @@ This file is part of the QGROUNDCONTROL project
 #include "QMap3DWidget.h"
 #include "SlugsDataSensorView.h"
 #include "LogCompressor.h"
+#include "slugshilsim.h"
 
 
 /**
@@ -180,6 +181,7 @@ protected:
     QPointer<QDockWidget> hsiDockWidget;
     QPointer<QDockWidget> rcViewDockWidget;
     QPointer<QDockWidget> slugsDataWidget;
+    QPointer<QDockWidget> slugsHilSimWidget;
 
     // Popup widgets
     JoystickWidget* joystickWidget;

src/ui/slugshilsim.cc

@@ -1,3 +1,33 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Configuration Window for Slugs' HIL Simulator
+ *   @author Mariano Lizarraga <malife@gmail.com>
+ */
+
+
 #include "slugshilsim.h"
 #include "ui_slugshilsim.h"
 #include "LinkManager.h"
@@ -7,8 +37,15 @@ SlugsHilSim::SlugsHilSim(QWidget *parent) :
     ui(new Ui::SlugsHilSim)
 {
     ui->setupUi(this);
-    linkAdded();
 
+    rxSocket = new QUdpSocket(this);
+    txSocket = new QUdpSocket(this);
+
+    connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addToCombo(LinkInterface*)));
+    connect(ui->bt_startHil, SIGNAL(clicked()), this, SLOT(putInHilMode()));
+    connect(rxSocket, SIGNAL(readyRead()), this, SLOT(readDatagram()));
+
+    linksAvailable.clear();
 }
 
 SlugsHilSim::~SlugsHilSim()
@@ -18,12 +55,74 @@ SlugsHilSim::~SlugsHilSim()
 
 void SlugsHilSim::linkAdded(void){
 
-  ui->cb_mavlinkLinks->clear();
+//  ui->cb_mavlinkLinks->clear();
+
+//  QList<LinkInterface *> linkList;
+//  linkList.append(LinkManager::instance()->getLinks()) ;
+
+//  for (int i = 0; i< linkList.size(); i++){
+//    ui->cb_mavlinkLinks->addItem((linkList.takeFirst())->getName());
+//  }
+
+}
+
+void SlugsHilSim::addToCombo(LinkInterface* theLink){
+
+  ui->cb_mavlinkLinks->addItem(theLink->getName());
+  linksAvailable.insert(ui->cb_mavlinkLinks->count(),theLink);
+}
+
+void SlugsHilSim::putInHilMode(void){
+
+  bool sw_enableControls = !(ui->bt_startHil->isChecked());
+  QString  buttonCaption= ui->bt_startHil->isChecked()? "Stop Slugs HIL Mode": "Set Slugs in HIL Mode";
+
+  if (ui->bt_startHil->isChecked()){
+    QMessageBox msgBox;
+    msgBox.setIcon(QMessageBox::Critical);
+    msgBox.setText("You are about to put SLUGS in HIL Mode.");
+    msgBox.setInformativeText("It will stop reading the actual sensor readings. Do you wish to continue?");
+    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No);
+    msgBox.setDefaultButton(QMessageBox::No);
+
+    if(msgBox.exec() == QMessageBox::Yes)
+    {
+      rxSocket->bind(QHostAddress::Any, ui->ed_rxPort->text().toInt());
+      txSocket->bind(QHostAddress::Broadcast, ui->ed_txPort->text().toInt());
+
+      ui->ed_ipAdress->setEnabled(sw_enableControls);
+      ui->ed_rxPort->setEnabled(sw_enableControls);
+      ui->ed_txPort->setEnabled(sw_enableControls);
+      ui->cb_mavlinkLinks->setEnabled(sw_enableControls);
 
-  QList<LinkInterface *> linkList = LinkManager::instance()->getLinks() ;
+      ui->bt_startHil->setText(buttonCaption);
 
-  for (int i = 0; i< linkList.size(); i++){
+
-    ui->cb_mavlinkLinks->addItem((linkList.takeFirst())->getName());
+    } else {
+      ui->bt_startHil->setChecked(false);
+    }
+  } else {
+    ui->ed_ipAdress->setEnabled(sw_enableControls);
+    ui->ed_rxPort->setEnabled(sw_enableControls);
+    ui->ed_txPort->setEnabled(sw_enableControls);
+    ui->cb_mavlinkLinks->setEnabled(sw_enableControls);
+
+    ui->bt_startHil->setText(buttonCaption);
   }
 
+
+
+
+}
+
+void SlugsHilSim::readDatagram(void){
+
+}
+
+
+void SlugsHilSim::activeUasSet(UASInterface* uas){
+
+  if (uas != NULL) {
+    activeUas = uas;
+  }
 }

src/ui/slugshilsim.h

@@ -1,10 +1,42 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the configuration Window for Slugs' HIL Simulator
+ *   @author Mariano Lizarraga <malife@gmail.com>
+ */
+
 #ifndef SLUGSHILSIM_H
 #define SLUGSHILSIM_H
 
 #include <QWidget>
 #include <QHostAddress>
 #include <QUdpSocket>
+#include <QMessageBox>
+
 #include "LinkInterface.h"
+#include "UAS.h"
 
 
 namespace Ui {
@@ -24,11 +56,52 @@ protected:
     QHostAddress* simulinkIp;
     QUdpSocket* txSocket;
     QUdpSocket* rxSocket;
+    UAS* activeUas;
+
+public slots:
+    void linkAdded (void);
+
+    /**
+     * @brief Adds a link to the combo box listing so the user can select a link
+     *
+     * Populates the Combo box that allows the user to select the link with which Slugs will
+     * receive the simulated sensor data from Simulink
+     *
+     * @param theLink the link that is being added to the combo box
+     */
+    void addToCombo(LinkInterface* theLink);
+
+    /**
+     * @brief Puts Slugs in HIL Mode
+     *
+     * Sends the required messages through the main communication link to set Slugs in HIL Mode
+     *
+     */
+    void putInHilMode(void);
+
+    /**
+     * @brief Receives a datagram from Simulink containing the sensor data.
+     *
+     * Receives a datagram from Simulink containing the simulated sensor data. This data is then
+     * forwarded to Slugs to use as input to the attitude estimation and navigation algorithms.
+     *
+     */
+    void readDatagram(void);
+
+    /**
+     * @brief Called when the a new UAS is set to active.
+     *
+     * Called when the a new UAS is set to active.
+     *
+     * @param uas The new active UAS
+     */
+    void activeUasSet(UASInterface* uas);
 
-slots:
 
 private:
     Ui::SlugsHilSim *ui;
+    QHash <int, LinkInterface*> linksAvailable;
+
 };
 
 #endif // SLUGSHILSIM_H

src/ui/slugshilsim.ui

@@ -6,19 +6,19 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>256</width>
+    <width>325</width>
     <height>191</height>
    </rect>
   </property>
   <property name="minimumSize">
    <size>
-    <width>256</width>
+    <width>320</width>
     <height>191</height>
    </size>
   </property>
   <property name="maximumSize">
    <size>
-    <width>367</width>
+    <width>450</width>
     <height>229</height>
    </size>
   </property>
@@ -45,7 +45,7 @@
           </font>
          </property>
          <property name="layoutDirection">
-          <enum>Qt::RightToLeft</enum>
+          <enum>Qt::LeftToRight</enum>
          </property>
          <property name="text">
           <string>IP Address</string>
@@ -91,7 +91,7 @@
           </font>
          </property>
          <property name="layoutDirection">
-          <enum>Qt::RightToLeft</enum>
+          <enum>Qt::LeftToRight</enum>
          </property>
          <property name="text">
           <string>Receive Port</string>
@@ -124,7 +124,7 @@
           </font>
          </property>
          <property name="layoutDirection">
-          <enum>Qt::RightToLeft</enum>
+          <enum>Qt::LeftToRight</enum>
          </property>
          <property name="text">
           <string>Send Port</string>
@@ -132,7 +132,7 @@
         </widget>
        </item>
        <item row="3" column="1">
-        <widget class="QLineEdit" name="ed_TxPort">
+        <widget class="QLineEdit" name="ed_txPort">
          <property name="minimumSize">
           <size>
            <width>60</width>
@@ -165,8 +165,8 @@
     </layout>
    </item>
    <item>
-    <layout class="QGridLayout" name="gridLayout_2">
+    <layout class="QHBoxLayout" name="horizontalLayout_3">
-     <item row="0" column="0">
+     <item>
       <widget class="QLabel" name="label_4">
        <property name="font">
         <font>
@@ -179,12 +179,32 @@
         <enum>Qt::RightToLeft</enum>
        </property>
        <property name="text">
-        <string>Slugs HIL Sim Serial Link</string>
+        <string>Slugs HIL Link</string>
        </property>
       </widget>
      </item>
-     <item row="0" column="1">
+     <item>
-      <widget class="QComboBox" name="cb_mavlinkLinks"/>
+      <spacer name="horizontalSpacer_3">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>40</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+     <item>
+      <widget class="QComboBox" name="cb_mavlinkLinks">
+       <property name="minimumSize">
+        <size>
+         <width>171</width>
+         <height>26</height>
+        </size>
+       </property>
+      </widget>
      </item>
     </layout>
    </item>
@@ -206,7 +226,10 @@
      <item>
       <widget class="QPushButton" name="bt_startHil">
        <property name="text">
-        <string>Set in HIL Mode</string>
+        <string>Set Slugs in HIL Mode</string>
+       </property>
+       <property name="checkable">
+        <bool>true</bool>
        </property>
       </widget>
      </item>