SlugsHilSim.cc 11.9 KB
Newer Older
Alejandro's avatar
Alejandro committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
/*=====================================================================

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>
 *   @author Alejandro Molina <am.alex09@gmail.com>
 */


#include "SlugsHilSim.h"
#include "ui_SlugsHilSim.h"


SlugsHilSim::SlugsHilSim(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::SlugsHilSim)
{
    ui->setupUi(this);

    rxSocket = new QUdpSocket(this);
    txSocket = new QUdpSocket(this);

    hilLink = NULL;

    connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addToCombo(LinkInterface*)));
    connect(ui->cb_mavlinkLinks, SIGNAL(currentIndexChanged(int)), this, SLOT(linkSelected(int)));
    connect(ui->bt_startHil, SIGNAL(clicked()), this, SLOT(putInHilMode()));
    connect(rxSocket, SIGNAL(readyRead()), this, SLOT(readDatagram()));

    linksAvailable.clear();

54
#ifdef MAVLINK_ENABLED_SLUGS
Alejandro's avatar
Alejandro committed
55
56
57
58
59
60
    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));
61
#endif
Alejandro's avatar
Alejandro committed
62

63
    foreach (LinkInterface* link, LinkManager::instance()->getLinks()) {
Alejandro's avatar
Alejandro committed
64
65
66
67
68
69
70
71
72
73
        addToCombo(link);
    }
}

SlugsHilSim::~SlugsHilSim()
{
    rxSocket->disconnectFromHost();
    delete ui;
}

74
75
void SlugsHilSim::addToCombo(LinkInterface* theLink)
{
Alejandro's avatar
Alejandro committed
76

77
78
    linksAvailable.insert(ui->cb_mavlinkLinks->count(),theLink);
    ui->cb_mavlinkLinks->addItem(theLink->getName());
Alejandro's avatar
Alejandro committed
79

80
81
82
    if (hilLink == NULL) {
        hilLink = theLink;
    }
Alejandro's avatar
Alejandro committed
83
84
85

}

86
87
void SlugsHilSim::putInHilMode(void)
{
Alejandro's avatar
Alejandro committed
88

89
90
    bool sw_enableControls = !(ui->bt_startHil->isChecked());
    QString  buttonCaption= ui->bt_startHil->isChecked()? "Stop Slugs HIL Mode": "Set Slugs in HIL Mode";
Alejandro's avatar
Alejandro committed
91

92
93
94
95
96
97
98
    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);
Alejandro's avatar
Alejandro committed
99

100
101
102
103
        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());
Alejandro's avatar
Alejandro committed
104

105
106
107
108
            ui->ed_ipAdress->setEnabled(sw_enableControls);
            ui->ed_rxPort->setEnabled(sw_enableControls);
            ui->ed_txPort->setEnabled(sw_enableControls);
            ui->cb_mavlinkLinks->setEnabled(sw_enableControls);
Alejandro's avatar
Alejandro committed
109

110
            ui->bt_startHil->setText(buttonCaption);
Alejandro's avatar
Alejandro committed
111

112
            activeUas->startHil();
113

114
115
116
        } else {
            ui->bt_startHil->setChecked(false);
        }
Alejandro's avatar
Alejandro committed
117
    } else {
118
119
120
121
        ui->ed_ipAdress->setEnabled(sw_enableControls);
        ui->ed_rxPort->setEnabled(sw_enableControls);
        ui->ed_txPort->setEnabled(sw_enableControls);
        ui->cb_mavlinkLinks->setEnabled(sw_enableControls);
Alejandro's avatar
Alejandro committed
122

123
        ui->bt_startHil->setText(buttonCaption);
Alejandro's avatar
Alejandro committed
124

125
126
127
        rxSocket->disconnectFromHost();
        activeUas->stopHil();
    }
Alejandro's avatar
Alejandro committed
128
129
}

130
131
132
133
134
135
136
137
void SlugsHilSim::readDatagram(void)
{
    static int count = 0;
    while (rxSocket->hasPendingDatagrams()) {
        QByteArray datagram;
        datagram.resize(rxSocket->pendingDatagramSize());
        QHostAddress sender;
        quint16 senderPort;
Alejandro's avatar
Alejandro committed
138

139
140
        rxSocket->readDatagram(datagram.data(), datagram.size(),
                               &sender, &senderPort);
Alejandro's avatar
Alejandro committed
141

142
143
        if (datagram.size() == 113) {
            processHilDatagram(&datagram);
Alejandro's avatar
Alejandro committed
144

145
            sendMessageToSlugs();
Alejandro's avatar
Alejandro committed
146

147
148
            commandDatagramToSimulink();
        }
Alejandro's avatar
Alejandro committed
149

150
151
        ui->ed_count->setText(QString::number(count++));
    }
Alejandro's avatar
Alejandro committed
152
153
154
}


155
156
void SlugsHilSim::activeUasSet(UASInterface* uas)
{
Alejandro's avatar
Alejandro committed
157

158
159
160
    if (uas != NULL) {
        activeUas = static_cast <UAS *>(uas);
    }
Alejandro's avatar
Alejandro committed
161
162
163
164
165
}


void SlugsHilSim::processHilDatagram(const QByteArray* datagram)
{
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
#ifdef MAVLINK_ENABLED_SLUGS
    unsigned char i = 0;


    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++);

    tmpGpsData.lat = getFloatFromDatagram(datagram, &i);
    tmpGpsData.lon = getFloatFromDatagram(datagram, &i);
    tmpGpsData.alt = getFloatFromDatagram(datagram, &i);

    tmpGpsData.hdg = getUint16FromDatagram(datagram, &i);
    tmpGpsData.v   = getUint16FromDatagram(datagram, &i);

    tmpGpsData.eph = getUint16FromDatagram(datagram, &i);
    tmpGpsData.fix_type = datagram->at(i++);
    tmpGpsTime.visSat  = datagram->at(i++);
    i++;

    tmpAirData.dynamicPressure= getFloatFromDatagram(datagram, &i);
    tmpAirData.staticPressure= getFloatFromDatagram(datagram, &i);
    tmpAirData.temperature= getUint16FromDatagram(datagram, &i);

    // TODO Salto en el Datagrama
    i=i+8;

    tmpRawImuData.xgyro = getUint16FromDatagram(datagram, &i);
    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);

    // TODO: this is legacy of old HIL datagram. Need to remove from Simulink model
    i++;

    ui->ed_1->setText(QString::number(tmpRawImuData.xacc));
    ui->ed_2->setText(QString::number(tmpRawImuData.yacc));
    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));
Alejandro's avatar
Alejandro committed
234
235

#else
236
    Q_UNUSED(datagram);
Alejandro's avatar
Alejandro committed
237
238
239
#endif
}

240
241
242
float SlugsHilSim::getFloatFromDatagram (const QByteArray* datagram, unsigned char * i)
{
    tFloatToChar tmpF2C;
Alejandro's avatar
Alejandro committed
243

244
245
246
247
    tmpF2C.chData[0] = datagram->at((*i)++);
    tmpF2C.chData[1] = datagram->at((*i)++);
    tmpF2C.chData[2] = datagram->at((*i)++);
    tmpF2C.chData[3] = datagram->at((*i)++);
Alejandro's avatar
Alejandro committed
248

249
    return tmpF2C.flData;
Alejandro's avatar
Alejandro committed
250
251
}

252
253
254
uint16_t SlugsHilSim::getUint16FromDatagram (const QByteArray* datagram, unsigned char * i)
{
    tUint16ToChar tmpU2C;
Alejandro's avatar
Alejandro committed
255

256
257
    tmpU2C.chData[0] = datagram->at((*i)++);
    tmpU2C.chData[1] = datagram->at((*i)++);
Alejandro's avatar
Alejandro committed
258

259
    return tmpU2C.uiData;
Alejandro's avatar
Alejandro committed
260
261
262
263
264

}

void SlugsHilSim::linkSelected(int cbIndex)
{
265
#ifdef MAVLINK_ENABLED_SLUGS
Alejandro's avatar
Alejandro committed
266
    // HIL code to go here...
267
    //hilLink = linksAvailable
Alejandro's avatar
Alejandro committed
268
269
270
271
272
273
    // FIXME Mariano

    hilLink =linksAvailable.value(cbIndex);

#else
    Q_UNUSED(cbIndex)
274
#endif
Alejandro's avatar
Alejandro committed
275
276
277
278
}

void SlugsHilSim::sendMessageToSlugs()
{
279
#ifdef MAVLINK_ENABLED_SLUGS
Alejandro's avatar
Alejandro committed
280
281
282
    mavlink_message_t msg;

    mavlink_msg_local_position_encode(MG::SYSTEM::ID,
283
284
285
                                      MG::SYSTEM::COMPID,
                                      &msg,
                                      &tmpLocalPositionData);
Alejandro's avatar
Alejandro committed
286
287
288
289
    activeUas->sendMessage(hilLink, msg);
    memset(&msg, 0, sizeof(mavlink_message_t));

    mavlink_msg_attitude_encode(MG::SYSTEM::ID,
290
291
292
                                MG::SYSTEM::COMPID,
                                &msg,
                                &tmpAttitudeData);
Alejandro's avatar
Alejandro committed
293
294
295
296
    activeUas->sendMessage(hilLink, msg);
    memset(&msg, 0, sizeof(mavlink_message_t));

    mavlink_msg_raw_imu_encode(MG::SYSTEM::ID,
297
298
299
                               MG::SYSTEM::COMPID,
                               &msg,
                               &tmpRawImuData);
Alejandro's avatar
Alejandro committed
300
301
302
303
    activeUas->sendMessage(hilLink, msg);
    memset(&msg, 0, sizeof(mavlink_message_t));

    mavlink_msg_air_data_encode(MG::SYSTEM::ID,
304
305
306
                                MG::SYSTEM::COMPID,
                                &msg,
                                &tmpAirData);
Alejandro's avatar
Alejandro committed
307
308
309
310
311
    activeUas->sendMessage(hilLink, msg);
    memset(&msg, 0, sizeof(mavlink_message_t));

    mavlink_msg_gps_raw_encode(MG::SYSTEM::ID,
                               MG::SYSTEM::COMPID,
312
313
                               &msg,
                               &tmpGpsData);
Alejandro's avatar
Alejandro committed
314
315
316
317
318
    activeUas->sendMessage(hilLink, msg);
    memset(&msg, 0, sizeof(mavlink_message_t));

    mavlink_msg_gps_date_time_encode(MG::SYSTEM::ID,
                                     MG::SYSTEM::COMPID,
319
320
                                     &msg,
                                     &tmpGpsTime);
Alejandro's avatar
Alejandro committed
321
322
323
324
325
326
327
328
    activeUas->sendMessage(hilLink, msg);
    memset(&msg, 0, sizeof(mavlink_message_t));
#endif
}


void SlugsHilSim::commandDatagramToSimulink()
{
329
#ifdef MAVLINK_ENABLED_SLUGS
Alejandro's avatar
Alejandro committed
330
331
    //mavlink_pwm_commands_t* pwdC = (static_cast<SlugsMAV*>(activeUas))->getPwmCommands();

332
    //mavlink_pwm_commands_t* pwdC;
Alejandro's avatar
Alejandro committed
333

334
335
//    if(pwdC != NULL){
//    }
Alejandro's avatar
Alejandro committed
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364

    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());
#endif
}

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];
}