Skip to content

Q
qgroundcontrol
Commit 30afdd85 authored by tecnosapiens's avatar tecnosapiens
Browse files
Options

test debugs

parents 348087f9 a3c9c11f
Hide whitespace changes
Inline Side-by-side
Showing with 221 additions and 14 deletions
src/uas/SlugsMAV.cc
View file @ 30afdd85
...@@ -299,6 +299,7 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -299,6 +299,7 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
} }
case MAVLINK_MSG_ID_ACTION_ACK: case MAVLINK_MSG_ID_ACTION_ACK:
{ {
qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_ACTION_ACK";
mavlink_action_ack_t ack; mavlink_action_ack_t ack;
mavlink_msg_action_ack_decode(&message,&ack); mavlink_msg_action_ack_decode(&message,&ack);
......
src/ui/SlugsPIDControl.cpp
View file @ 30afdd85
...@@ -19,14 +19,9 @@ SlugsPIDControl::SlugsPIDControl(QWidget *parent) : ...@@ -19,14 +19,9 @@ SlugsPIDControl::SlugsPIDControl(QWidget *parent) :
activeUAS = NULL; activeUAS = NULL;
setRedColorStyle(); setRedColorStyle();
setGreenColorStyle(); setGreenColorStyle();
// connect_set_pushButtons(); // connect_set_pushButtons();
// connect_AirSpeed_LineEdit(); // connect_AirSpeed_LineEdit();
// connect_PitchFollowei_LineEdit(); // connect_PitchFollowei_LineEdit();
......
src/ui/slugshilsim.cc
View file @ 30afdd85
...@@ -50,6 +50,7 @@ SlugsHilSim::SlugsHilSim(QWidget *parent) : ...@@ -50,6 +50,7 @@ SlugsHilSim::SlugsHilSim(QWidget *parent) :
SlugsHilSim::~SlugsHilSim() SlugsHilSim::~SlugsHilSim()
{ {
rxSocket->disconnectFromHost();
delete ui; delete ui;
} }
...@@ -87,8 +88,9 @@ void SlugsHilSim::putInHilMode(void){ ...@@ -87,8 +88,9 @@ void SlugsHilSim::putInHilMode(void){
if(msgBox.exec() == QMessageBox::Yes) if(msgBox.exec() == QMessageBox::Yes)
{ {
rxSocket->disconnectFromHost();
rxSocket->bind(QHostAddress::Any, ui->ed_rxPort->text().toInt()); 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_ipAdress->setEnabled(sw_enableControls);
ui->ed_rxPort->setEnabled(sw_enableControls); ui->ed_rxPort->setEnabled(sw_enableControls);
...@@ -108,21 +110,100 @@ void SlugsHilSim::putInHilMode(void){ ...@@ -108,21 +110,100 @@ void SlugsHilSim::putInHilMode(void){
ui->cb_mavlinkLinks->setEnabled(sw_enableControls); ui->cb_mavlinkLinks->setEnabled(sw_enableControls);
ui->bt_startHil->setText(buttonCaption); 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;
if (uas != NULL) {
//activeUas = uas;
}
} }
src/ui/slugshilsim.h
View file @ 30afdd85
...@@ -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 "LinkInterface.h"
#include "UAS.h" #include "UAS.h"
#include <stdint.h>
namespace Ui { namespace Ui {
...@@ -103,9 +104,24 @@ public slots: ...@@ -103,9 +104,24 @@ public slots:
private: 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; Ui::SlugsHilSim *ui;
QHash <int, LinkInterface*> linksAvailable; 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 #endif // SLUGSHILSIM_H
src/ui/slugshilsim.ui
View file @ 30afdd85
...@@ -7,19 +7,19 @@ ...@@ -7,19 +7,19 @@
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>325</width> <width>325</width>
<height>191</height> <height>278</height>
</rect> </rect>
</property> </property>
<property name="minimumSize"> <property name="minimumSize">
<size> <size>
<width>320</width> <width>320</width>
<height>191</height> <height>252</height>
</size> </size>
</property> </property>
<property name="maximumSize"> <property name="maximumSize">
<size> <size>
<width>450</width> <width>450</width>
<height>229</height> <height>278</height>
</size> </size>
</property> </property>
<property name="font"> <property name="font">
...@@ -235,6 +235,120 @@ ...@@ -235,6 +235,120 @@
</item> </item>
</layout> </layout>
</item> </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> </layout>
</widget> </widget>
<resources/> <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