zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

First succesful test of HIL Sim. Getting ready to add the remaining messages

QGC4.4
Mariano Lizarraga 15 years ago
parent
03b9312ba9
commit
a3c9c11fa6
3 changed files with 214 additions and 9 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 87
      src/ui/slugshilsim.cc
  2. 16
      src/ui/slugshilsim.h
  3. 120
      src/ui/slugshilsim.ui

87
src/ui/slugshilsim.cc
Unescape View File

@ -50,6 +50,7 @@ SlugsHilSim::SlugsHilSim(QWidget *parent) : @@ -50,6 +50,7 @@ SlugsHilSim::SlugsHilSim(QWidget *parent) :
SlugsHilSim::~SlugsHilSim()
{
rxSocket->disconnectFromHost();
delete ui;
}
@ -87,8 +88,9 @@ void SlugsHilSim::putInHilMode(void){ @@ -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){ @@ -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 = uas;
activeUas = static_cast <UAS *>(uas);
}
}
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;
tmpF2C.chData[0] = datagram->at((*i)++);
tmpF2C.chData[1] = datagram->at((*i)++);
tmpF2C.chData[2] = datagram->at((*i)++);
tmpF2C.chData[3] = datagram->at((*i)++);
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;
}

16
src/ui/slugshilsim.h
Unescape View File

@ -37,6 +37,7 @@ This file is part of the QGROUNDCONTROL project @@ -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: @@ -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

120
src/ui/slugshilsim.ui
Unescape View File

@ -7,19 +7,19 @@ @@ -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 @@ @@ -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/>

Write Preview
Loading…
Cancel
Save