You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
347 lines
11 KiB
347 lines
11 KiB
/*===================================================================== |
|
|
|
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> |
|
*/ |
|
|
|
|
|
#include "SlugsHilSim.h" |
|
#include "ui_SlugsHilSim.h" |
|
#include "LinkManager.h" |
|
|
|
#include "SlugsMAV.h" |
|
#include "qbytearray.h" |
|
#include "qhostaddress.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(); |
|
|
|
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)); |
|
} |
|
|
|
SlugsHilSim::~SlugsHilSim() |
|
{ |
|
rxSocket->disconnectFromHost(); |
|
delete ui; |
|
} |
|
|
|
void SlugsHilSim::addToCombo(LinkInterface* theLink){ |
|
|
|
ui->cb_mavlinkLinks->addItem(theLink->getName()); |
|
linksAvailable.insert(ui->cb_mavlinkLinks->count(),theLink); |
|
|
|
if (hilLink == NULL){ |
|
hilLink = theLink; |
|
} |
|
|
|
} |
|
|
|
void SlugsHilSim::putInHilMode(void){ |
|
|
|
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); |
|
|
|
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()); |
|
|
|
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); |
|
|
|
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); |
|
|
|
sendMessageToSlugs(); |
|
|
|
commandDatagramToSimulink(); |
|
} |
|
|
|
ui->ed_count->setText(QString::number(count++)); |
|
} |
|
} |
|
|
|
|
|
void SlugsHilSim::activeUasSet(UASInterface* uas){ |
|
|
|
if (uas != NULL) { |
|
activeUas = static_cast <UAS *>(uas); |
|
} |
|
} |
|
|
|
|
|
void SlugsHilSim::processHilDatagram(const QByteArray* datagram) |
|
{ |
|
#ifdef MAVLINK_ENABLED_SLUGS |
|
unsigned char i = 0; |
|
|
|
mavlink_message_t msg; |
|
|
|
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); |
|
|
|
|
|
// 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 |
|
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)); |
|
|
|
#else |
|
Q_UNUSED(datagram); |
|
#endif |
|
} |
|
|
|
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; |
|
|
|
} |
|
|
|
void SlugsHilSim::linkSelected(int cbIndex){ |
|
//hilLink = linksAvailable |
|
// FIXME Mariano |
|
} |
|
|
|
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; |
|
//pwdC->dt_c = 1; |
|
|
|
QByteArray data; |
|
data.resize(22); |
|
|
|
unsigned char i=0; |
|
setUInt16ToDatagram(data, &i, rand());//pwdC->dt_c); |
|
setUInt16ToDatagram(data, &i, rand());//pwdC->dla_c); |
|
setUInt16ToDatagram(data, &i, rand());//pwdC->dra_c); |
|
setUInt16ToDatagram(data, &i, rand());//pwdC->dr_c); |
|
setUInt16ToDatagram(data, &i, rand());//pwdC->dle_c); |
|
setUInt16ToDatagram(data, &i, rand());//pwdC->dre_c); |
|
setUInt16ToDatagram(data, &i, rand());//pwdC->dlf_c); |
|
setUInt16ToDatagram(data, &i, rand());//pwdC->drf_c); |
|
setUInt16ToDatagram(data, &i, rand());//pwdC->aux1); |
|
setUInt16ToDatagram(data, &i, rand());//pwdC->aux2); |
|
setUInt16ToDatagram(data, &i, rand());//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]; |
|
}
|
|
|