slugshilsim.cc 6.16 KB
Newer Older
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
/*=====================================================================

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>
 */


31 32
#include "slugshilsim.h"
#include "ui_slugshilsim.h"
33
#include "LinkManager.h"
34 35 36 37 38 39

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

41 42 43
    rxSocket = new QUdpSocket(this);
    txSocket = new QUdpSocket(this);

44 45
    hilLink = NULL;

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

    linksAvailable.clear();
52 53 54 55
}

SlugsHilSim::~SlugsHilSim()
{
56
    rxSocket->disconnectFromHost();
57 58
    delete ui;
}
59

60 61 62 63
void SlugsHilSim::addToCombo(LinkInterface* theLink){

  ui->cb_mavlinkLinks->addItem(theLink->getName());
  linksAvailable.insert(ui->cb_mavlinkLinks->count(),theLink);
64 65 66 67 68

  if (hilLink == NULL){
    hilLink = theLink;
  }

69
}
70

71
void SlugsHilSim::putInHilMode(void){
72

73 74 75 76 77 78 79 80 81 82 83
  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);

84
    if(msgBox.exec() == QMessageBox::Yes) {
85
      rxSocket->disconnectFromHost();
86
      rxSocket->bind(QHostAddress::Any, ui->ed_rxPort->text().toInt());
87
      //txSocket->bind(QHostAddress::Broadcast, ui->ed_txPort->text().toInt());
88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105

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

    } 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);
106 107

    rxSocket->disconnectFromHost();
108
  }
109
}
110

111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128
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++));
       }
}
129 130


131
void SlugsHilSim::activeUasSet(UASInterface* uas){
132

133 134 135
  if (uas != NULL) {
    activeUas = static_cast <UAS *>(uas);
  }
136 137 138
}


139 140 141 142 143 144 145
void SlugsHilSim::processHilDatagram(const QByteArray* datagram){
  unsigned char i = 0;

  mavlink_message_t msg;

  // GPS
  mavlink_gps_raw_t tmpGpsRaw;
146 147

#ifdef MAVLINK_ENABLED_SLUGS
148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167
  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++);

168
  mavlink_msg_gps_date_time_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsTime);
169
  activeUas->sendMessage(hilLink, msg);
170
#endif
171 172

  memset(&msg, 0, sizeof(mavlink_message_t));
173

174
  mavlink_msg_gps_raw_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsRaw);
175
  activeUas->sendMessage(hilLink,msg);
176 177 178 179 180 181 182

  // 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));
183 184
}

185 186
float SlugsHilSim::getFloatFromDatagram (const QByteArray* datagram, unsigned char * i){
  tFloatToChar tmpF2C;
187

188 189 190 191
  tmpF2C.chData[0] = datagram->at((*i)++);
  tmpF2C.chData[1] = datagram->at((*i)++);
  tmpF2C.chData[2] = datagram->at((*i)++);
  tmpF2C.chData[3] = datagram->at((*i)++);
192

193 194 195 196 197 198 199 200 201 202
  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;
203

204
}
205 206 207 208

void SlugsHilSim::linkSelected(int cbIndex){
  //hilLink = linksAvailable
}