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

Merge branch 'master' of git://github.com/amolinap/qgroundcontrol into mergeRemote

QGC4.4
Mariano Lizarraga 14 years ago
parent
cbc50b8c85 2881eaf2d7
commit
3656dc6d27
6 changed files with 228 additions and 26 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      src/uas/SlugsMAV.cc
  2. 5
      src/uas/SlugsMAV.h
  3. 21
      src/ui/MainWindow.cc
  4. 191
      src/ui/SlugsHilSim.cc
  5. 16
      src/ui/SlugsHilSim.h
  6. 15
      src/ui/SlugsHilSim.ui

6
src/uas/SlugsMAV.cc
Unescape View File

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

5
src/uas/SlugsMAV.h
Unescape View File

@ -36,12 +36,15 @@ class SlugsMAV : public UAS
public: public:
SlugsMAV(MAVLinkProtocol* mavlink, int id = 0); SlugsMAV(MAVLinkProtocol* mavlink, int id = 0);
public slots: public slots:
/** @brief Receive a MAVLink message from this MAV */ /** @brief Receive a MAVLink message from this MAV */
void receiveMessage(LinkInterface* link, mavlink_message_t message); void receiveMessage(LinkInterface* link, mavlink_message_t message);
void emitSignals (void); void emitSignals (void);
mavlink_pwm_commands_t* getPwmCommands();
signals: signals:
@ -70,6 +73,8 @@ signals:
#endif #endif
protected: protected:

21
src/ui/MainWindow.cc
Unescape View File

@ -292,6 +292,21 @@ void MainWindow::buildCommonWidgets()
addToCentralWidgetsMenu (protocolWidget, "Mavlink Generator", SLOT(showCentralWidget()),CENTRAL_PROTOCOL); 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);
}
if (!dataplotWidget) if (!dataplotWidget)
{ {
dataplotWidget = new QGCDataPlot2D(this); dataplotWidget = new QGCDataPlot2D(this);
@ -798,6 +813,12 @@ void MainWindow::connectCommonWidgets()
// it notifies that a waypoint global goes to do create and a map graphic too // 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))); 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() void MainWindow::createCustomWidget()

191
src/ui/SlugsHilSim.cc
Unescape View File

@ -32,6 +32,10 @@ This file is part of the QGROUNDCONTROL project
#include "ui_SlugsHilSim.h" #include "ui_SlugsHilSim.h"
#include "LinkManager.h" #include "LinkManager.h"
#include "SlugsMAV.h"
#include "qbytearray.h"
#include "qhostaddress.h"
SlugsHilSim::SlugsHilSim(QWidget *parent) : SlugsHilSim::SlugsHilSim(QWidget *parent) :
QWidget(parent), QWidget(parent),
ui(new Ui::SlugsHilSim) ui(new Ui::SlugsHilSim)
@ -49,6 +53,18 @@ SlugsHilSim::SlugsHilSim(QWidget *parent) :
connect(rxSocket, SIGNAL(readyRead()), this, SLOT(readDatagram())); connect(rxSocket, SIGNAL(readyRead()), this, SLOT(readDatagram()));
linksAvailable.clear(); 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));
foreach (LinkInterface* link, LinkManager::instance()->getLinks())
{
addToCombo(link);
}
} }
SlugsHilSim::~SlugsHilSim() SlugsHilSim::~SlugsHilSim()
@ -121,6 +137,10 @@ void SlugsHilSim::readDatagram(void){
if (datagram.size() == 113) { if (datagram.size() == 113) {
processHilDatagram(&datagram); processHilDatagram(&datagram);
sendMessageToSlugs();
commandDatagramToSimulink();
} }
ui->ed_count->setText(QString::number(count++)); ui->ed_count->setText(QString::number(count++));
@ -143,11 +163,6 @@ void SlugsHilSim::processHilDatagram(const QByteArray* datagram)
mavlink_message_t msg; mavlink_message_t msg;
// GPS
mavlink_gps_raw_t tmpGpsRaw;
mavlink_gps_date_time_t tmpGpsTime;
tmpGpsTime.year = datagram->at(i++); tmpGpsTime.year = datagram->at(i++);
tmpGpsTime.month = datagram->at(i++); tmpGpsTime.month = datagram->at(i++);
tmpGpsTime.day = datagram->at(i++); tmpGpsTime.day = datagram->at(i++);
@ -155,31 +170,73 @@ void SlugsHilSim::processHilDatagram(const QByteArray* datagram)
tmpGpsTime.min = datagram->at(i++); tmpGpsTime.min = datagram->at(i++);
tmpGpsTime.sec = datagram->at(i++); tmpGpsTime.sec = datagram->at(i++);
tmpGpsRaw.lat = getFloatFromDatagram(datagram, &i); tmpGpsData.lat = getFloatFromDatagram(datagram, &i);
tmpGpsRaw.lon = getFloatFromDatagram(datagram, &i); tmpGpsData.lon = getFloatFromDatagram(datagram, &i);
tmpGpsRaw.alt = getFloatFromDatagram(datagram, &i); tmpGpsData.alt = getFloatFromDatagram(datagram, &i);
tmpGpsRaw.hdg = getUint16FromDatagram(datagram, &i); tmpGpsData.hdg = getUint16FromDatagram(datagram, &i);
tmpGpsRaw.v = getUint16FromDatagram(datagram, &i); tmpGpsData.v = getUint16FromDatagram(datagram, &i);
tmpGpsRaw.eph = getUint16FromDatagram(datagram, &i);
tmpGpsRaw.fix_type = datagram->at(i++); tmpGpsData.eph = getUint16FromDatagram(datagram, &i);
tmpGpsData.fix_type = datagram->at(i++);
tmpGpsTime.visSat = datagram->at(i++); tmpGpsTime.visSat = datagram->at(i++);
i++;
mavlink_msg_gps_date_time_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsTime); tmpAirData.dynamicPressure= getFloatFromDatagram(datagram, &i);
activeUas->sendMessage(hilLink, msg); tmpAirData.staticPressure= getFloatFromDatagram(datagram, &i);
tmpAirData.temperature= getUint16FromDatagram(datagram, &i);
memset(&msg, 0, sizeof(mavlink_message_t)); // TODO Salto en el Datagrama
i=i+8;
mavlink_msg_gps_raw_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsRaw); tmpRawImuData.xgyro = getUint16FromDatagram(datagram, &i);
activeUas->sendMessage(hilLink,msg); 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);
// TODO Crear Paquete SYNC TIME
i=i+2;
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_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 // TODO: this is legacy of old HIL datagram. Need to remove from Simulink model
i++; i++;
ui->ed_1->setText(QString::number(tmpGpsRaw.hdg)); ui->ed_1->setText(QString::number(tmpRawImuData.xacc));
ui->ed_2->setText(QString::number(tmpGpsRaw.v)); ui->ed_2->setText(QString::number(tmpRawImuData.yacc));
ui->ed_3->setText(QString::number(tmpGpsRaw.eph)); 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 #else
Q_UNUSED(datagram); Q_UNUSED(datagram);
#endif #endif
@ -209,9 +266,93 @@ uint16_t SlugsHilSim::getUint16FromDatagram (const QByteArray* datagram, unsigne
void SlugsHilSim::linkSelected(int cbIndex){ void SlugsHilSim::linkSelected(int cbIndex){
#ifdef MAVLINK_ENABLED_SLUGS #ifdef MAVLINK_ENABLED_SLUGS
// HIL code to go here... // HIL code to go here...
//hilLink = linksAvailable //hilLink = linksAvailable
Q_UNUSED(cbIndex); // FIXME Mariano
#else
Q_UNUSED(cbIndex); hilLink = LinkManager::instance()->getLinkForId(cbIndex);
#endif }
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::commandDatagramToSimulink()
{
//mavlink_pwm_commands_t* pwdC = (static_cast<SlugsMAV*>(activeUas))->getPwmCommands();
mavlink_pwm_commands_t* pwdC;
if(pwdC != NULL){
}
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);//value default
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];
} }

16
src/ui/SlugsHilSim.h
Unescape View File

@ -52,6 +52,8 @@ public:
explicit SlugsHilSim(QWidget *parent = 0); explicit SlugsHilSim(QWidget *parent = 0);
~SlugsHilSim(); ~SlugsHilSim();
protected: protected:
LinkInterface* hilLink; LinkInterface* hilLink;
QHostAddress* simulinkIp; QHostAddress* simulinkIp;
@ -59,6 +61,13 @@ protected:
QUdpSocket* rxSocket; QUdpSocket* rxSocket;
UAS* activeUas; 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: public slots:
/** /**
@ -121,11 +130,18 @@ private:
} tUint16ToChar; } tUint16ToChar;
Ui::SlugsHilSim *ui; Ui::SlugsHilSim *ui;
QHash <int, LinkInterface*> linksAvailable; QHash <int, LinkInterface*> linksAvailable;
void processHilDatagram (const QByteArray* datagram); void processHilDatagram (const QByteArray* datagram);
float getFloatFromDatagram (const QByteArray* datagram, unsigned char * i); float getFloatFromDatagram (const QByteArray* datagram, unsigned char * i);
uint16_t getUint16FromDatagram (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 commandDatagramToSimulink();
}; };

15
src/ui/SlugsHilSim.ui
Unescape View File

@ -6,7 +6,7 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>325</width> <width>337</width>
<height>278</height> <height>278</height>
</rect> </rect>
</property> </property>
@ -349,6 +349,19 @@
</item> </item>
</layout> </layout>
</item> </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> </layout>
</widget> </widget>
<resources/> <resources/>

Write Preview
Loading…
Cancel
Save