From 2d477d0650f88a3e5ad17f8e03036e624c94f354 Mon Sep 17 00:00:00 2001 From: Pos <pierpaolo.mancini@mail.polimi.it> Date: Wed, 14 Jul 2021 15:17:43 +0200 Subject: [PATCH] Implemented log in mavlink moduel --- Modules/Graph/graphmodule.cpp | 12 +- Modules/Graph/graphmodule.h | 5 + Modules/Mavlink/mavlinkcommandadapter.cpp | 19 +- Modules/Mavlink/mavlinkcommandadapter.h | 7 +- Modules/Mavlink/mavlinkmodule.cpp | 174 +++-- Modules/Mavlink/mavlinkmodule.h | 6 +- Modules/Mavlink/mavlinkmodule.ui | 93 ++- Modules/Mavlink/mavlinkreader.cpp | 154 +++- Modules/Mavlink/mavlinkreader.h | 22 +- Modules/Mavlink/mavlinkwriter.cpp | 21 +- Modules/Mavlink/mavlinkwriter.h | 7 +- Modules/Mavlink/rs232.c | 824 ++++++++++++++++++++++ Modules/Mavlink/rs232.h | 87 +++ Modules/skywardhubstrings.cpp | 2 + Modules/skywardhubstrings.h | 2 + SkywardHub.pro | 2 + SkywardHub.pro.user | 2 +- 17 files changed, 1260 insertions(+), 179 deletions(-) create mode 100644 Modules/Mavlink/rs232.c create mode 100644 Modules/Mavlink/rs232.h diff --git a/Modules/Graph/graphmodule.cpp b/Modules/Graph/graphmodule.cpp index b47ed636..6ecf3395 100644 --- a/Modules/Graph/graphmodule.cpp +++ b/Modules/Graph/graphmodule.cpp @@ -13,6 +13,9 @@ GraphModule::GraphModule(QWidget *parent) : DefaultModule(parent), ui(new Ui::Gr defaultContextMenuSetup(); buildCentralGraphView(); ui->centralLayout->addWidget(&graphCentralView); + connect(&updaterTimer, &QTimer::timeout, this, &GraphModule::onUpdateTimerTick); + updaterTimer.setSingleShot(false); + updaterTimer.start(updatePeriod); } GraphModule::~GraphModule() @@ -87,7 +90,12 @@ void GraphModule::setTheme() // } // } // graphCentralView.setBackground(QBrush(themeBackgroundColor)); -// replotAll(); + // replotAll(); +} + +void GraphModule::onUpdateTimerTick() +{ + replotAll(); } QCPGraph *GraphModule::instantiateNewGraph() @@ -186,7 +194,7 @@ void GraphModule::onMsgReceived(const ModuleMessage &msg) if(ok && graph != nullptr){ graph->addData(x,y); //Da rimuovere per performance - replotAll(); + //replotAll(); } } } diff --git a/Modules/Graph/graphmodule.h b/Modules/Graph/graphmodule.h index 0003e2e8..95baf59d 100644 --- a/Modules/Graph/graphmodule.h +++ b/Modules/Graph/graphmodule.h @@ -7,6 +7,7 @@ #include "Core/modulemessage.h" #include "qcustomplot.h" #include "Modules/DefaultModule/defaultmodule.h" +#include <QTimer> namespace Ui { class GraphModule; @@ -41,6 +42,8 @@ protected: void addCustomActionsToMenu() override; QCPGraph *getSelectedGraph(); void setTheme(); + void onUpdateTimerTick(); + private: Ui::GraphModule *ui; QCustomPlot graphCentralView; @@ -49,6 +52,8 @@ private: int followedGraphIndex = -1; QString dateFormat = "HH:mm:ss\n(zzz)"; //QSharedPointer<QCPAxisTickerDateTime> dateTicker; + QTimer updaterTimer; + int updatePeriod = 1000; // [ms] }; #endif // GRAPHMODULE_H diff --git a/Modules/Mavlink/mavlinkcommandadapter.cpp b/Modules/Mavlink/mavlinkcommandadapter.cpp index 539374ab..f42cb853 100644 --- a/Modules/Mavlink/mavlinkcommandadapter.cpp +++ b/Modules/Mavlink/mavlinkcommandadapter.cpp @@ -9,10 +9,10 @@ MavlinkCommandAdapter::MavlinkCommandAdapter() } -void MavlinkCommandAdapter::setSerialPort(QSerialPort *value) -{ - serial = value; -} +//void MavlinkCommandAdapter::setSerialPort(QSerialPort *value) +//{ +// serial = value; +//} mavlink_message_t MavlinkCommandAdapter::encode_PING_TC() { @@ -133,8 +133,9 @@ void MavlinkCommandAdapter::send(mavlink_message_t msg) response.addOption(SkywardHubStrings::msgIdField, QString::number(msg.msgid)); response.addOption(SkywardHubStrings::msgSequenceNumberField, QString::number(msg.seq)); - if(serial != nullptr && serial->isOpen()){ - mavlinkWriter.startAsyncWrite(serial,msg); + if(portNumber >= 0){ + mavlinkWriter.setPortNumber(portNumber); + mavlinkWriter.startAsyncWrite(msg); } else{ response.setPayload(SkywardHubStrings::serialPortClosedErrorMsg); @@ -143,6 +144,12 @@ void MavlinkCommandAdapter::send(mavlink_message_t msg) emit publishRequested(response); } +void MavlinkCommandAdapter::setPortNumber(int value) +{ + portNumber = value; +} + + //void MavlinkCommandAdapter::loadMsgDefinition() //{ // XmlObject mavlinkMsgDef; diff --git a/Modules/Mavlink/mavlinkcommandadapter.h b/Modules/Mavlink/mavlinkcommandadapter.h index ffd20cd2..7d3a2ac6 100644 --- a/Modules/Mavlink/mavlinkcommandadapter.h +++ b/Modules/Mavlink/mavlinkcommandadapter.h @@ -21,7 +21,8 @@ public: MavlinkCommandAdapter(); void send(mavlink_message_t msg); - void setSerialPort(QSerialPort *value); + // void setSerialPort(QSerialPort *value); + void setPortNumber(int port); mavlink_message_t encode_PING_TC(); mavlink_message_t encode_NOARG_TC(int cmd_id); @@ -44,13 +45,15 @@ protected: //void parseMavTmList(const XmlObject *xml); private: - QSerialPort *serial = nullptr; + // QSerialPort *serial = nullptr; MavlinkWriter mavlinkWriter; QMap<QString, int> msgNameIdMap; //QString msgDefinitionFile = GroundStation::getResourceDirectory()+"/mavlinkMsgDefinition.xml"; static const int MAV_CMP = 1; static const int MAV_SYS = 1; + + int portNumber = -1; }; #endif // MAVLINKCOMMANDADAPTER_H diff --git a/Modules/Mavlink/mavlinkmodule.cpp b/Modules/Mavlink/mavlinkmodule.cpp index 3980fb11..16bc1aab 100644 --- a/Modules/Mavlink/mavlinkmodule.cpp +++ b/Modules/Mavlink/mavlinkmodule.cpp @@ -5,6 +5,7 @@ #include <QSerialPortInfo> #include "Core/modulemessagesbroker.h" #include "Modules/skywardhubstrings.h" +#include "rs232.h" MavlinkModule::MavlinkModule(QWidget *parent) : DefaultModule(parent), ui(new Ui::MavlinkModule) { @@ -18,7 +19,7 @@ MavlinkModule::MavlinkModule(QWidget *parent) : DefaultModule(parent), ui(new Ui connect(&mavlinkReader,&MavlinkReader::msgReceived, this, &MavlinkModule::onMsgReceived); connect(&mavlinkCommandAdapter,&MavlinkCommandAdapter::publishRequested, this, &MavlinkModule::publish); connect(&linkQualityTimer, &QTimer::timeout, this, &MavlinkModule::onLinkQualityTimerTick); - mavlinkCommandAdapter.setSerialPort(&serialPort); +// mavlinkCommandAdapter.setSerialPort(&serialPort); subscribe(); } @@ -26,7 +27,7 @@ MavlinkModule::~MavlinkModule() { mavlinkReader.stopReading(); delete ui; - closePort(); + onStopClicked(); } QWidget* MavlinkModule::toWidget() @@ -69,26 +70,17 @@ void MavlinkModule::publish(const ModuleMessage &msg) void MavlinkModule::closePort() { - if(serialPort.isOpen()){ - serialPort.close(); - } + if(portOpen && portNumber >= 0){ + // rs232 code + portOpen = false; + RS232_CloseComport(portNumber); + return; + } +// if(serialPort.isOpen()){ +// serialPort.close(); +// } } -//void MavlinkModule::replotAll() -//{ -// if(ui->linkQuality_groupBox->isChecked()){ -// if(ui->checkBox_follow->isChecked() && linkGraph != nullptr && !linkGraph->data()->isEmpty()){ -// // Center the view to the last sample in the graph -// double lastKey = (linkGraph->data()->constEnd()-1)->key; -// double lastValue = (linkGraph->data()->constEnd()-1)->value; -// double size_x = graphCentralView.xAxis->range().size(); -// double size_y = graphCentralView.yAxis->range().size(); -// graphCentralView.xAxis->setRange(lastKey, size_x, Qt::AlignmentFlag::AlignCenter); -// graphCentralView.yAxis->setRange(lastValue, size_y, Qt::AlignmentFlag::AlignCenter); -// } -// graphCentralView.replot(); -// } -//} void MavlinkModule::initializeSerialPortView() { @@ -121,90 +113,52 @@ void MavlinkModule::initializeSerialPortView() } -//void MavlinkModule::buildCentralGraphView() -//{ -// ui->graphLayout->addWidget(&graphCentralView); -// linkGraph = graphCentralView.addGraph(); - -// // Set graph style -// linkGraph->setPen(QPen(Qt::green)); // line color -// linkGraph->setBrush(QBrush(QColor(0, 128, 0, 40))); // first graph will be filled with this color - - -// // int maxPackets = ui->spinBox_maxPackets->value(); -// // graphCentralView.yAxis->setRange(0, maxPackets); - -// graphCentralView.yAxis2->setVisible(false); -// // graphCentralView.yAxis2->setTicks(true); -// // graphCentralView.yAxis2->setTickLabels(true); - -// //graphCentralView.xAxis2->setVisible(true); -// //textTicker = QSharedPointer<QCPAxisTickerText>(new QCPAxisTickerText()); -// //cPlot.xAxis2->setTicker(textTicker); -// //graphCentralView.xAxis2->setTickLabels(true); - -// QSharedPointer<QCPAxisTickerDateTime> dateTimeTicker(new QCPAxisTickerDateTime); -// graphCentralView.xAxis->setTicker(dateTimeTicker); -// //QDateTime rangeLowerBound = QDateTime::currentDateTime().addDays(-2); -// double now = QDateTime::currentDateTime().toTime_t(); -// QDateTime rangeUpperBound = QDateTime::currentDateTime().addDays(+2); -// graphCentralView.xAxis->setRange(now, QCPAxisTickerDateTime::dateTimeToKey(rangeUpperBound)); -// dateTimeTicker->setDateTimeFormat(dateFormat); - -// graphCentralView.legend->setVisible(false); - -// // make left and bottom axes always transfer their ranges to right and top axes: -// connect(graphCentralView.xAxis, SIGNAL(rangeChanged(QCPRange)), graphCentralView.xAxis2, SLOT(setRange(QCPRange))); -// connect(graphCentralView.yAxis, SIGNAL(rangeChanged(QCPRange)), graphCentralView.yAxis2, SLOT(setRange(QCPRange))); - -// // Allow user to drag axis ranges with mouse, zoom with mouse wheel and select graphs by clicking: -// graphCentralView.setInteractions(QCP::iRangeDrag | QCP::iRangeZoom | QCP::iSelectPlottables); - -// // Disable drag and zoom on y axe -// // graphCentralView.axisRects().at(0)->setRangeDrag(Qt::Horizontal); -// // graphCentralView.axisRects().at(0)->setRangeZoom(Qt::Horizontal); - -// // Enable context menu -// graphCentralView.setContextMenuPolicy(Qt::CustomContextMenu); -// // connect(&graphCentralView,SIGNAL(customContextMenuRequested(QPoint)),this,SLOT(onCustomContextMenuRequested(QPoint))); -// connect(&graphCentralView, &QCustomPlot::customContextMenuRequested, this, &MavlinkModule::onCustomContextMenuRequested); -// //connect(customPlot,SIGNAL(selectionChangedByUser()),this,SLOT(selectedGraphChange())); -//} - - bool MavlinkModule::startReadingOnSerialPort() { QString portName = ui->lineEdit_serialPort->text().trimmed(); bool ok; int baudRate = ui->comboBox_baudRate->currentData().toInt(&ok); - if(ok && portName != "" && !serialPort.isOpen()){ - QSerialPortInfo port(portName); - serialPort.setPort(port); - serialPort.setBaudRate(baudRate); -// serialPort.setDataBits(QSerialPort::Data8); -// serialPort.setParity(QSerialPort::NoParity); -// serialPort.setStopBits(QSerialPort::TwoStop); - return serialPort.open(QIODevice::ReadWrite); - } - return false; -} -void MavlinkModule::onStartStreamToggled(bool state) -{ - onLinkQualityIndicatorClicked(ui->linkQuality_groupBox->isChecked()); + QString portNumberChar = portName.remove("COM"); + std::string mode = "8N1"; + bool ok2; + int number = portNumberChar.toInt(&ok2); + if(ok && ok2 && !portOpen){ + portNumber = number - 1; + if (RS232_OpenComport(portNumber,baudRate, mode.c_str())){ + QString errorMsg = "Mavlink >> Error, can't open serial port"; + qDebug() << errorMsg; + portOpen = -1; + } + else{ + // All good - if(state){ - onStartClicked(); - } - else{ - onStopClicked(); + return true; + } } + + + // QSerialport code +// if(ok && portName != "" && !serialPort.isOpen()){ +// QSerialPortInfo port(portName); +// serialPort.setPort(port); +// serialPort.setBaudRate(baudRate); +//// serialPort.setDataBits(QSerialPort::Data8); +//// serialPort.setParity(QSerialPort::NoParity); +//// serialPort.setStopBits(QSerialPort::OneStop); +// return serialPort.open(QIODevice::ReadWrite); +// } + return false; } void MavlinkModule::onStartClicked() { if(startReadingOnSerialPort()){ - mavlinkReader.startAsyncReading(&serialPort); + portOpen = true; + mavlinkReader.setPortNumber(portNumber); + mavlinkReader.startAsyncReading(); + mavlinkCommandAdapter.setPortNumber(portNumber); + //mavlinkReader.startAsyncReadingWithSignal(&serialPort); } } @@ -220,6 +174,18 @@ void MavlinkModule::onMsgReceived(const ModuleMessage &msg) publish(msg); } +void MavlinkModule::onStartStreamToggled(bool state) +{ + onLinkQualityIndicatorClicked(ui->linkQuality_groupBox->isChecked()); + + if(state){ + onStartClicked(); + } + else{ + onStopClicked(); + } +} + void MavlinkModule::onLinkQualityIndicatorClicked(bool checked) { if(checked && !linkQualityTimer.isActive()){ @@ -247,6 +213,22 @@ void MavlinkModule::updateLinkSignalIndicator(int msgArrived, float ratio) publish(linkQualityMsgRatio); } + + +void MavlinkModule::onOpenLogFolderClick() +{ + QDir dir(SkywardHubStrings::defaultLogsFolder); + if (dir.exists()){ + QDesktopServices::openUrl( QUrl::fromLocalFile(SkywardHubStrings::defaultLogsFolder) ); + } + else{ + QString msg = "The log folder does not exist yet.\n\nIt will be created when opening the serial port.\n\n" + SkywardHubStrings::defaultLogsFolder; + QMessageBox msgBox; + msgBox.setText(msg); + msgBox.exec(); + } +} + void MavlinkModule::connectUiSlots() { ui->startStop_button_layout->insertWidget(0, &startSerialStreamToggleButton); @@ -255,6 +237,7 @@ void MavlinkModule::connectUiSlots() // connect(ui->button_start,&QPushButton::clicked,this, &MavlinkModule::onStartClicked); // connect(ui->button_stop,&QPushButton::clicked,this, &MavlinkModule::onStopClicked); // connect(ui->spinBox_maxPackets,SIGNAL(valueChanged(int)),this,SLOT(onMaxPackedValueChanged(int))); + connect(ui->openLogFolder_button, &QPushButton::clicked, this, &MavlinkModule::onOpenLogFolderClick); connect(ui->comboBox_serialPort, SIGNAL(currentIndexChanged(int)), this, SLOT(onSerialPortIndexChanged(int))); connect(ui->linkQuality_groupBox, &QGroupBox::clicked, this, &MavlinkModule::onLinkQualityIndicatorClicked); } @@ -407,7 +390,10 @@ void MavlinkModule::onCommandReceived(const ModuleMessage &msg) // } // } - mavlinkCommandAdapter.send(encoded_mvl_msg); + if(portOpen && portNumber >= 0){ + mavlinkCommandAdapter.send(encoded_mvl_msg); + } + } void MavlinkModule::onTelemetryRequestReceived(const ModuleMessage &msg) @@ -469,5 +455,7 @@ void MavlinkModule::onTelemetryRequestReceived(const ModuleMessage &msg) encoded_mvl_msg = mavlinkCommandAdapter.encode_TELEMETRY_REQUEST_TC(MAV_SENSORS_TM_ID); } - mavlinkCommandAdapter.send(encoded_mvl_msg); + if(portOpen && portNumber >= 0){ + mavlinkCommandAdapter.send(encoded_mvl_msg); + } } diff --git a/Modules/Mavlink/mavlinkmodule.h b/Modules/Mavlink/mavlinkmodule.h index fe8e652f..59895461 100644 --- a/Modules/Mavlink/mavlinkmodule.h +++ b/Modules/Mavlink/mavlinkmodule.h @@ -54,6 +54,7 @@ protected: void updateLinkSignalIndicator(); void updateLinkSignalIndicator(int msgArrived, float ratio); + void onOpenLogFolderClick(); private slots: void onSerialPortIndexChanged(int index); @@ -70,12 +71,13 @@ private: QString dateFormat = "HH:mm:ss\n(zzz)"; MavlinkReader mavlinkReader; MavlinkCommandAdapter mavlinkCommandAdapter; - QSerialPort serialPort; + //QSerialPort serialPort; QTimer linkQualityTimer; int linkQualityPeriod = 1000; // [ms] int msgArrived = 0; - + bool portOpen = false; + int portNumber = -1; }; #endif // MAVLINKMODULE_H diff --git a/Modules/Mavlink/mavlinkmodule.ui b/Modules/Mavlink/mavlinkmodule.ui index a37cbb82..3b7b057c 100644 --- a/Modules/Mavlink/mavlinkmodule.ui +++ b/Modules/Mavlink/mavlinkmodule.ui @@ -77,6 +77,9 @@ <property name="minimum"> <number>1</number> </property> + <property name="maximum"> + <number>99999</number> + </property> </widget> </item> <item row="1" column="2"> @@ -104,6 +107,32 @@ <set>Qt::AlignCenter</set> </property> <layout class="QGridLayout" name="gridLayout"> + <item row="1" column="0"> + <spacer name="horizontalSpacer_3"> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + <property name="sizeHint" stdset="0"> + <size> + <width>40</width> + <height>20</height> + </size> + </property> + </spacer> + </item> + <item row="1" column="7"> + <spacer name="horizontalSpacer_2"> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + <property name="sizeHint" stdset="0"> + <size> + <width>40</width> + <height>20</height> + </size> + </property> + </spacer> + </item> <item row="1" column="2"> <widget class="QLineEdit" name="lineEdit_serialPort"> <property name="sizePolicy"> @@ -114,19 +143,6 @@ </property> </widget> </item> - <item row="1" column="5"> - <layout class="QHBoxLayout" name="startStop_button_layout"/> - </item> - <item row="0" column="5"> - <widget class="QLabel" name="label_6"> - <property name="text"> - <string>Start/Stop</string> - </property> - <property name="alignment"> - <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set> - </property> - </widget> - </item> <item row="0" column="1"> <widget class="QLabel" name="label_2"> <property name="text"> @@ -147,8 +163,8 @@ </property> </widget> </item> - <item row="1" column="4"> - <widget class="QDoubleSpinBox" name="doubleSpinBox"/> + <item row="1" column="5"> + <layout class="QHBoxLayout" name="startStop_button_layout"/> </item> <item row="0" column="3"> <widget class="QLabel" name="label_3"> @@ -157,54 +173,29 @@ </property> </widget> </item> - <item row="1" column="1"> - <widget class="QComboBox" name="comboBox_serialPort"/> - </item> - <item row="0" column="4"> - <widget class="QLabel" name="label"> - <property name="sizePolicy"> - <sizepolicy hsizetype="Preferred" vsizetype="Fixed"> - <horstretch>0</horstretch> - <verstretch>0</verstretch> - </sizepolicy> - </property> + <item row="0" column="5"> + <widget class="QLabel" name="label_6"> <property name="text"> - <string>Timeout [s]</string> + <string>Start/Stop</string> </property> <property name="alignment"> - <set>Qt::AlignCenter</set> + <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set> </property> </widget> </item> <item row="1" column="6"> - <spacer name="horizontalSpacer_2"> - <property name="orientation"> - <enum>Qt::Horizontal</enum> - </property> - <property name="sizeHint" stdset="0"> - <size> - <width>40</width> - <height>20</height> - </size> + <widget class="QPushButton" name="openLogFolder_button"> + <property name="text"> + <string>Log Folder</string> </property> - </spacer> + </widget> + </item> + <item row="1" column="1"> + <widget class="QComboBox" name="comboBox_serialPort"/> </item> <item row="1" column="3"> <widget class="QComboBox" name="comboBox_baudRate"/> </item> - <item row="1" column="0"> - <spacer name="horizontalSpacer_3"> - <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> </widget> </item> diff --git a/Modules/Mavlink/mavlinkreader.cpp b/Modules/Mavlink/mavlinkreader.cpp index d908f4bf..0c3f6ac4 100644 --- a/Modules/Mavlink/mavlinkreader.cpp +++ b/Modules/Mavlink/mavlinkreader.cpp @@ -2,8 +2,11 @@ #include <QFile> #include <QTextStream> - +#include <QDebug> #include "windows.h" +#include "rs232.h" +#include "Modules/skywardhubstrings.h" +#include <QDir> MavlinkReader::MavlinkReader() { @@ -12,15 +15,23 @@ MavlinkReader::MavlinkReader() MavlinkReader::~MavlinkReader() { + closeLog(); +} +void MavlinkReader::startAsyncReading() +{ + stop = false; + openLogFile(); + this->start(); // Chiama run() } -void MavlinkReader::startAsyncReading(QSerialPort *port) +void MavlinkReader::startAsyncReadingWithSignal(QSerialPort *port) { stop = false; serial = port; - this->start(); // Chiama run() + connect(serial, &QSerialPort::readyRead, this, &MavlinkReader::handleReadyRead); + connect(serial, &QSerialPort::errorOccurred, this, &MavlinkReader::handleError); } //void MavlinkReader::loadMsgMapping() @@ -40,36 +51,84 @@ void MavlinkReader::startAsyncReading(QSerialPort *port) void MavlinkReader::run() { + bool ok; while (!stop) { - mavlink_message_t msg = waitForMavlinkMsg(); - parseMavlinkMsg(&msg); + mavlink_message_t msg = waitForMavlinkMsg(ok); + if(ok){ + parseMavlinkMsg(&msg); + } } } +void MavlinkReader::setPortNumber(int value) +{ + portNumber = value; +} + void MavlinkReader::stopReading() { stop = true; + closeLog(); } -mavlink_message_t MavlinkReader::waitForMavlinkMsg() +mavlink_message_t MavlinkReader::waitForMavlinkMsg(bool &ok) { mavlink_message_t msg; + + //ok = readWithQSerialPort(&msg); + ok = readWithRS232(&msg); + + return msg; +} + +bool MavlinkReader::readWithRS232(mavlink_message_t *msg) +{ mavlink_status_t status; - int waitingTimeout = 1000; + unsigned char byte; int numRead = 0; + while (!stop) { + numRead = RS232_PollComport(portNumber,&byte,1); + //qDebug() << byte; + + if(numRead > 0){ + + if(logFile.isOpen()) + { + const char c = byte; + logFile.write(&c, sizeof(char)); + } + + // add byte to parser + if(mavlink_parse_char(MAVLINK_COMM_0, byte, msg, &status)){ + // if the parser has found a message, return it + return true; + } + } + } + return false; +} + +bool MavlinkReader::readWithQSerialPort(mavlink_message_t *msg) +{ + mavlink_status_t status; + int waitingTimeout = 1000; unsigned char byte; + int numRead = 0; + while (!stop && serial != nullptr && serial->waitForReadyRead(waitingTimeout)) { + QByteArray dataRead = serial->readAll(); numRead = dataRead.size(); - for(int i = 0; i < dataRead.count(); i++){ byte = dataRead.at(i); - if(mavlink_parse_char(MAVLINK_COMM_0, byte, &msg, &status)){ +// qDebug() << "MavReader: " << byte; + + if(mavlink_parse_char(MAVLINK_COMM_0, byte, msg, &status)){ // if the parser has found a message, return it - return msg; + return true; } } @@ -79,7 +138,35 @@ mavlink_message_t MavlinkReader::waitForMavlinkMsg() } } - return msg; + return false; +} + + +void MavlinkReader::handleReadyRead() +{ + + if(stop) + return; + + mavlink_message_t msg; + mavlink_status_t status; + + + QByteArray dataRead = serial->readAll(); + int numRead = dataRead.count(); + for(int i = 0; i < numRead; i++){ + unsigned char byte = dataRead.at(i); + + if(mavlink_parse_char(MAVLINK_COMM_0, byte, &msg, &status)){ + // if the parser has found a message, return it + parseMavlinkMsg(&msg); + } + } +} + +void MavlinkReader::handleError() +{ + qDebug() << "MavlinkReader >> Serial read error"; } QList<ModuleMessage> MavlinkReader::parseMavlinkMsg(mavlink_message_t *msg) @@ -145,7 +232,7 @@ QList<ModuleMessage> MavlinkReader::translateToGsMessage(mavlink_message_t *msg, QList<ModuleMessage> messagesReceived; for(QString fieldName : payloads.keys()){ - QString topic = msgName + "/" + fieldName; + QString topic = SkywardHubStrings::mavlink_received_msg_topic + "/" + msgName + "/" + fieldName; ModuleMessage msg(topic, payloads[fieldName],timestamp); emit msgReceived(msg); messagesReceived.append(msg); @@ -153,6 +240,48 @@ QList<ModuleMessage> MavlinkReader::translateToGsMessage(mavlink_message_t *msg, return messagesReceived; } +void MavlinkReader::closeLog() +{ + if(logFile.isOpen()) + logFile.close(); +} + +void MavlinkReader::openLogFile() +{ + closeLog(); + + setLogFilePath(); + + logFile.setFileName(logFilePath); + if (logFile.open(QIODevice::WriteOnly)){ + qDebug() << "MavlinkReader >> Opened " << logFilePath; + QTextStream out(&logFile); + out << "Log started " << QDateTime::currentDateTime().toString() << "\n"; + } + else{ + qDebug() << "MavlinkReader >> Error, can not open " << logFilePath; + } +} + +void MavlinkReader::setLogFilePath() +{ + QDir dir(SkywardHubStrings::defaultLogsFolder); + if (!dir.exists()) + dir.mkpath("."); + + QString extension = ".dat"; + QString currentFile = QDateTime::currentDateTime().toString("yyyyMMdd_hh.mm.ss"); + QString proposedFilePath = SkywardHubStrings::defaultLogsFolder + "/" + currentFile; + QString fileNumber = ""; + int i = 1; + while(QFile::exists(proposedFilePath+fileNumber+extension) && i < 200){ + i++; + fileNumber = " ("+QString::number(i)+")"; + } + + logFilePath = proposedFilePath+fileNumber+extension; +} + QString MavlinkReader::extractOneField(mavlink_message_t *msg, const mavlink_field_info_t *f, int idx) { @@ -243,4 +372,3 @@ QString MavlinkReader::extractOneField(mavlink_message_t *msg, const mavlink_fie } - diff --git a/Modules/Mavlink/mavlinkreader.h b/Modules/Mavlink/mavlinkreader.h index c7085253..f6d40f6f 100644 --- a/Modules/Mavlink/mavlinkreader.h +++ b/Modules/Mavlink/mavlinkreader.h @@ -8,6 +8,8 @@ #include "mavlinkversionheader.h" #include "Core/modulemessage.h" +#include <QFile> + class MavlinkReader : public QThread { @@ -17,21 +19,34 @@ public: MavlinkReader(); ~MavlinkReader(); - void startAsyncReading(QSerialPort *port); + void startAsyncReading(); + void startAsyncReadingWithSignal(QSerialPort *port); + QList<ModuleMessage> parseMavlinkMsg(mavlink_message_t *msg); void stopReading(); + void setPortNumber(int value); + void closeLog(); + void openLogFile(); + signals: //void receivedMsg(const QString &topic,const QString &payload,const QDateTime ×tamp); void msgReceived(const ModuleMessage &msg); protected: //void loadMsgMapping(); - mavlink_message_t waitForMavlinkMsg(); + mavlink_message_t waitForMavlinkMsg(bool &ok); QString extractFields(mavlink_message_t *msg, const mavlink_field_info_t *f); QString extractOneField(mavlink_message_t *msg, const mavlink_field_info_t *f, int idx); + bool readWithQSerialPort(mavlink_message_t *msg); + bool readWithRS232(mavlink_message_t *msg); + + void handleReadyRead(); + void handleError(); + + void setLogFilePath(); /* * This method emit the signal msgReceived @@ -43,9 +58,12 @@ private: QSerialPort *serial = nullptr; bool stop = false; const mavlink_message_info_t message_info[256] = MAVLINK_MESSAGE_INFO; + int portNumber = -1; //XmlObject msgDefinition; //QString mavlinkMsgDefinitionFilePath = "GroundStationConfig/mavlinkMsgDefinition.xml"; + QString logFilePath = ""; + QFile logFile; }; #endif // MAVLINKREADER_H diff --git a/Modules/Mavlink/mavlinkwriter.cpp b/Modules/Mavlink/mavlinkwriter.cpp index 3467a677..bcc72ada 100644 --- a/Modules/Mavlink/mavlinkwriter.cpp +++ b/Modules/Mavlink/mavlinkwriter.cpp @@ -1,15 +1,16 @@ #include "mavlinkwriter.h" #include <chrono> +#include "rs232.h" +#include <QDebug> MavlinkWriter::MavlinkWriter() { } -void MavlinkWriter::startAsyncWrite(QSerialPort *port, mavlink_message_t msgToSend) +void MavlinkWriter::startAsyncWrite(mavlink_message_t msgToSend) { - serial = port; this->msgToSend = msgToSend; this->start(); // Call run() } @@ -19,18 +20,28 @@ void MavlinkWriter::writeMsg(mavlink_message_t *msgToSend) mtx.lock(); unsigned char buff[sizeof(mavlink_message_t)+1]; int msg_len = mavlink_msg_to_send_buffer(buff, msgToSend); - serial->write(reinterpret_cast<char*>(buff),msg_len); + //serial->write(reinterpret_cast<char*>(buff),msg_len); + try { + RS232_SendBuf(portNumber, buff, msg_len); + } catch (...) { + qDebug() << "MavlinkWriter: Error, writeMsg serial port error"; + } + mtx.unlock(); } void MavlinkWriter::run() { - if(serial != nullptr && serial->isOpen()){ + if(portNumber >= 0){ try { writeMsg(&msgToSend); } catch (...) { } } - serial = nullptr; +} + +void MavlinkWriter::setPortNumber(int value) +{ + portNumber = value; } diff --git a/Modules/Mavlink/mavlinkwriter.h b/Modules/Mavlink/mavlinkwriter.h index e82da7bd..f23471b8 100644 --- a/Modules/Mavlink/mavlinkwriter.h +++ b/Modules/Mavlink/mavlinkwriter.h @@ -13,14 +13,17 @@ class MavlinkWriter : public QThread public: MavlinkWriter(); - void startAsyncWrite(QSerialPort *port, mavlink_message_t msgToSend); + void startAsyncWrite(mavlink_message_t msgToSend); + + void setPortNumber(int value); protected: void writeMsg(mavlink_message_t *msgToSend); void run(); private: - QSerialPort *serial = nullptr; +// QSerialPort *serial = nullptr; + int portNumber = -1; mavlink_message_t msgToSend; std::mutex mtx; }; diff --git a/Modules/Mavlink/rs232.c b/Modules/Mavlink/rs232.c new file mode 100644 index 00000000..8f148844 --- /dev/null +++ b/Modules/Mavlink/rs232.c @@ -0,0 +1,824 @@ +/* +*************************************************************************** +* +* Author: Teunis van Beelen +* +* Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017 Teunis van Beelen +* +* Email: teuniz@gmail.com +* +*************************************************************************** +* +* This program 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. +* +* This program 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 this program. If not, see <http://www.gnu.org/licenses/>. +* +*************************************************************************** +*/ + + +/* Last revision: November 22, 2017 */ + +/* For more info and how to use this library, visit: http://www.teuniz.net/RS-232/ */ + + +#include "rs232.h" + + +#if defined(__linux__) || defined(__FreeBSD__) || defined(__APPLE__) /* Linux & FreeBSD */ + +#define RS232_PORTNR 39 + + +int Cport[RS232_PORTNR], + error; + +struct termios new_port_settings, + old_port_settings[RS232_PORTNR]; + +char *comports[RS232_PORTNR]={"/dev/tty.usbserial-1420","/dev/ttyS0","/dev/ttyS1","/dev/ttyS2","/dev/ttyS3","/dev/ttyS4","/dev/ttyS5", + "/dev/ttyS6","/dev/ttyS7","/dev/ttyS8","/dev/ttyS9","/dev/ttyS10","/dev/ttyS11", + "/dev/ttyS12","/dev/ttyS13","/dev/ttyS14","/dev/ttyS15","/dev/ttyUSB0", + "/dev/ttyUSB1","/dev/ttyUSB2","/dev/ttyUSB3","/dev/ttyUSB4","/dev/ttyUSB5", + "/dev/ttyAMA0","/dev/ttyAMA1","/dev/ttyACM0","/dev/ttyACM1", + "/dev/rfcomm0","/dev/rfcomm1","/dev/ircomm0","/dev/ircomm1", + "/dev/cuau0","/dev/cuau1","/dev/cuau2","/dev/cuau3", + "/dev/cuaU0","/dev/cuaU1","/dev/cuaU2","/dev/cuaU3"}; + +int RS232_OpenComport(int comport_number, int baudrate, const char *mode) +{ + int baudr, + status; + + if((comport_number>=RS232_PORTNR)||(comport_number<0)) + { + printf("illegal comport number\n"); + return(1); + } + + switch(baudrate) + { + case 50 : baudr = B50; + break; + case 75 : baudr = B75; + break; + case 110 : baudr = B110; + break; + case 134 : baudr = B134; + break; + case 150 : baudr = B150; + break; + case 200 : baudr = B200; + break; + case 300 : baudr = B300; + break; + case 600 : baudr = B600; + break; + case 1200 : baudr = B1200; + break; + case 1800 : baudr = B1800; + break; + case 2400 : baudr = B2400; + break; + case 4800 : baudr = B4800; + break; + case 9600 : baudr = B9600; + break; + case 19200 : baudr = B19200; + break; + case 38400 : baudr = B38400; + break; + case 57600 : baudr = B57600; + break; + case 115200 : baudr = B115200; + break; + case 230400 : baudr = B230400; + break; +#ifndef __APPLE__ + case 460800 : baudr = B460800; + break; + case 500000 : baudr = B500000; + break; + case 576000 : baudr = B576000; + break; + case 921600 : baudr = B921600; + break; + case 1000000 : baudr = B1000000; + break; + case 1152000 : baudr = B1152000; + break; + case 1500000 : baudr = B1500000; + break; + case 2000000 : baudr = B2000000; + break; + case 2500000 : baudr = B2500000; + break; + case 3000000 : baudr = B3000000; + break; + case 3500000 : baudr = B3500000; + break; + case 4000000 : baudr = B4000000; + break; +#endif + default : printf("invalid baudrate\n"); + return(1); + break; + } + + int cbits=CS8, + cpar=0, + ipar=IGNPAR, + bstop=0; + + if(strlen(mode) != 3) + { + printf("invalid mode \"%s\"\n", mode); + return(1); + } + + switch(mode[0]) + { + case '8': cbits = CS8; + break; + case '7': cbits = CS7; + break; + case '6': cbits = CS6; + break; + case '5': cbits = CS5; + break; + default : printf("invalid number of data-bits '%c'\n", mode[0]); + return(1); + break; + } + + switch(mode[1]) + { + case 'N': + case 'n': cpar = 0; + ipar = IGNPAR; + break; + case 'E': + case 'e': cpar = PARENB; + ipar = INPCK; + break; + case 'O': + case 'o': cpar = (PARENB | PARODD); + ipar = INPCK; + break; + default : printf("invalid parity '%c'\n", mode[1]); + return(1); + break; + } + + switch(mode[2]) + { + case '1': bstop = 0; + break; + case '2': bstop = CSTOPB; + break; + default : printf("invalid number of stop bits '%c'\n", mode[2]); + return(1); + break; + } + +/* +http://pubs.opengroup.org/onlinepubs/7908799/xsh/termios.h.html + +http://man7.org/linux/man-pages/man3/termios.3.html +*/ + + Cport[comport_number] = open(comports[comport_number], O_RDWR | O_NOCTTY | O_NDELAY); + if(Cport[comport_number]==-1) + { + perror("unable to open comport "); + return(1); + } + + /* lock access so that another process can't also use the port */ +#ifndef __APPLE__ + if(flock(Cport[comport_number], LOCK_EX | LOCK_NB) != 0) + { + close(Cport[comport_number]); + perror("Another process has locked the comport."); + return(1); + } +#endif + + error = tcgetattr(Cport[comport_number], old_port_settings + comport_number); + if(error==-1) + { + close(Cport[comport_number]); + flock(Cport[comport_number], LOCK_UN); /* free the port so that others can use it. */ + perror("unable to read portsettings "); + return(1); + } + memset(&new_port_settings, 0, sizeof(new_port_settings)); /* clear the new struct */ + + new_port_settings.c_cflag = cbits | cpar | bstop | CLOCAL | CREAD; + new_port_settings.c_iflag = ipar; + new_port_settings.c_oflag = 0; + new_port_settings.c_lflag = 0; + new_port_settings.c_cc[VMIN] = 0; /* block untill n bytes are received */ + new_port_settings.c_cc[VTIME] = 0; /* block untill a timer expires (n * 100 mSec.) */ + + cfsetispeed(&new_port_settings, baudr); + cfsetospeed(&new_port_settings, baudr); + + error = tcsetattr(Cport[comport_number], TCSANOW, &new_port_settings); + if(error==-1) + { + tcsetattr(Cport[comport_number], TCSANOW, old_port_settings + comport_number); + close(Cport[comport_number]); + flock(Cport[comport_number], LOCK_UN); /* free the port so that others can use it. */ + perror("unable to adjust portsettings "); + return(1); + } + +/* http://man7.org/linux/man-pages/man4/tty_ioctl.4.html */ + + if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) + { + tcsetattr(Cport[comport_number], TCSANOW, old_port_settings + comport_number); + flock(Cport[comport_number], LOCK_UN); /* free the port so that others can use it. */ + perror("unable to get portstatus"); + return(1); + } + + status |= TIOCM_DTR; /* turn on DTR */ + status |= TIOCM_RTS; /* turn on RTS */ + + if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) + { + tcsetattr(Cport[comport_number], TCSANOW, old_port_settings + comport_number); + flock(Cport[comport_number], LOCK_UN); /* free the port so that others can use it. */ + perror("unable to set portstatus"); + return(1); + } + + return(0); +} + + +int RS232_PollComport(int comport_number, unsigned char *buf, int size) +{ + int n; + + n = read(Cport[comport_number], buf, size); + + if(n < 0) + { + if(errno == EAGAIN) return 0; + } + + return(n); +} + + +int RS232_SendByte(int comport_number, unsigned char byte) +{ + int n = write(Cport[comport_number], &byte, 1); + if(n < 0) + { + if(errno == EAGAIN) + { + return 0; + } + else + { + return 1; + } + } + + return(0); +} + + +int RS232_SendBuf(int comport_number, unsigned char *buf, int size) +{ + int n = write(Cport[comport_number], buf, size); + if(n < 0) + { + if(errno == EAGAIN) + { + return 0; + } + else + { + return -1; + } + } + + return(n); +} + + +void RS232_CloseComport(int comport_number) +{ + int status; + + if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) + { + perror("unable to get portstatus"); + } + + status &= ~TIOCM_DTR; /* turn off DTR */ + status &= ~TIOCM_RTS; /* turn off RTS */ + + if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) + { + perror("unable to set portstatus"); + } + + tcsetattr(Cport[comport_number], TCSANOW, old_port_settings + comport_number); + close(Cport[comport_number]); + + flock(Cport[comport_number], LOCK_UN); /* free the port so that others can use it. */ +} + +/* +Constant Description +TIOCM_LE DSR (data set ready/line enable) +TIOCM_DTR DTR (data terminal ready) +TIOCM_RTS RTS (request to send) +TIOCM_ST Secondary TXD (transmit) +TIOCM_SR Secondary RXD (receive) +TIOCM_CTS CTS (clear to send) +TIOCM_CAR DCD (data carrier detect) +TIOCM_CD see TIOCM_CAR +TIOCM_RNG RNG (ring) +TIOCM_RI see TIOCM_RNG +TIOCM_DSR DSR (data set ready) + +http://man7.org/linux/man-pages/man4/tty_ioctl.4.html +*/ + +int RS232_IsDCDEnabled(int comport_number) +{ + int status; + + ioctl(Cport[comport_number], TIOCMGET, &status); + + if(status&TIOCM_CAR) return(1); + else return(0); +} + + +int RS232_IsCTSEnabled(int comport_number) +{ + int status; + + ioctl(Cport[comport_number], TIOCMGET, &status); + + if(status&TIOCM_CTS) return(1); + else return(0); +} + + +int RS232_IsDSREnabled(int comport_number) +{ + int status; + + ioctl(Cport[comport_number], TIOCMGET, &status); + + if(status&TIOCM_DSR) return(1); + else return(0); +} + + +void RS232_enableDTR(int comport_number) +{ + int status; + + if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) + { + perror("unable to get portstatus"); + } + + status |= TIOCM_DTR; /* turn on DTR */ + + if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) + { + perror("unable to set portstatus"); + } +} + + +void RS232_disableDTR(int comport_number) +{ + int status; + + if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) + { + perror("unable to get portstatus"); + } + + status &= ~TIOCM_DTR; /* turn off DTR */ + + if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) + { + perror("unable to set portstatus"); + } +} + + +void RS232_enableRTS(int comport_number) +{ + int status; + + if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) + { + perror("unable to get portstatus"); + } + + status |= TIOCM_RTS; /* turn on RTS */ + + if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) + { + perror("unable to set portstatus"); + } +} + + +void RS232_disableRTS(int comport_number) +{ + int status; + + if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) + { + perror("unable to get portstatus"); + } + + status &= ~TIOCM_RTS; /* turn off RTS */ + + if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) + { + perror("unable to set portstatus"); + } +} + + +void RS232_flushRX(int comport_number) +{ + tcflush(Cport[comport_number], TCIFLUSH); +} + + +void RS232_flushTX(int comport_number) +{ + tcflush(Cport[comport_number], TCOFLUSH); +} + + +void RS232_flushRXTX(int comport_number) +{ + tcflush(Cport[comport_number], TCIOFLUSH); +} + + +#else /* windows */ + +#define RS232_PORTNR 16 + +HANDLE Cport[RS232_PORTNR]; + + +char *comports[RS232_PORTNR]={"\\\\.\\COM1", "\\\\.\\COM2", "\\\\.\\COM3", "\\\\.\\COM4", + "\\\\.\\COM5", "\\\\.\\COM6", "\\\\.\\COM7", "\\\\.\\COM8", + "\\\\.\\COM9", "\\\\.\\COM10", "\\\\.\\COM11", "\\\\.\\COM12", + "\\\\.\\COM13", "\\\\.\\COM14", "\\\\.\\COM15", "\\\\.\\COM16"}; + +char mode_str[128]; + + +int RS232_OpenComport(int comport_number, int baudrate, const char *mode) +{ + if((comport_number>=RS232_PORTNR)||(comport_number<0)) + { + printf("illegal comport number\n"); + return(1); + } + + switch(baudrate) + { + case 110 : strcpy(mode_str, "baud=110"); + break; + case 300 : strcpy(mode_str, "baud=300"); + break; + case 600 : strcpy(mode_str, "baud=600"); + break; + case 1200 : strcpy(mode_str, "baud=1200"); + break; + case 2400 : strcpy(mode_str, "baud=2400"); + break; + case 4800 : strcpy(mode_str, "baud=4800"); + break; + case 9600 : strcpy(mode_str, "baud=9600"); + break; + case 19200 : strcpy(mode_str, "baud=19200"); + break; + case 38400 : strcpy(mode_str, "baud=38400"); + break; + case 57600 : strcpy(mode_str, "baud=57600"); + break; + case 115200 : strcpy(mode_str, "baud=115200"); + break; + case 128000 : strcpy(mode_str, "baud=128000"); + break; + case 256000 : strcpy(mode_str, "baud=256000"); + break; + case 500000 : strcpy(mode_str, "baud=500000"); + break; + case 1000000 : strcpy(mode_str, "baud=1000000"); + break; + default : printf("invalid baudrate\n"); + return(1); + break; + } + + if(strlen(mode) != 3) + { + printf("invalid mode \"%s\"\n", mode); + return(1); + } + + switch(mode[0]) + { + case '8': strcat(mode_str, " data=8"); + break; + case '7': strcat(mode_str, " data=7"); + break; + case '6': strcat(mode_str, " data=6"); + break; + case '5': strcat(mode_str, " data=5"); + break; + default : printf("invalid number of data-bits '%c'\n", mode[0]); + return(1); + break; + } + + switch(mode[1]) + { + case 'N': + case 'n': strcat(mode_str, " parity=n"); + break; + case 'E': + case 'e': strcat(mode_str, " parity=e"); + break; + case 'O': + case 'o': strcat(mode_str, " parity=o"); + break; + default : printf("invalid parity '%c'\n", mode[1]); + return(1); + break; + } + + switch(mode[2]) + { + case '1': strcat(mode_str, " stop=1"); + break; + case '2': strcat(mode_str, " stop=2"); + break; + default : printf("invalid number of stop bits '%c'\n", mode[2]); + return(1); + break; + } + + strcat(mode_str, " dtr=on rts=on"); + +/* +http://msdn.microsoft.com/en-us/library/windows/desktop/aa363145%28v=vs.85%29.aspx + +http://technet.microsoft.com/en-us/library/cc732236.aspx +*/ + + Cport[comport_number] = CreateFileA(comports[comport_number], + GENERIC_READ|GENERIC_WRITE, + 0, /* no share */ + NULL, /* no security */ + OPEN_EXISTING, + 0, /* no threads */ + NULL); /* no templates */ + + if(Cport[comport_number]==INVALID_HANDLE_VALUE) + { + printf("unable to open comport\n"); + return(1); + } + + DCB port_settings; + memset(&port_settings, 0, sizeof(port_settings)); /* clear the new struct */ + port_settings.DCBlength = sizeof(port_settings); + + if(!BuildCommDCBA(mode_str, &port_settings)) + { + printf("unable to set comport dcb settings\n"); + CloseHandle(Cport[comport_number]); + return(1); + } + + if(!SetCommState(Cport[comport_number], &port_settings)) + { + printf("unable to set comport cfg settings\n"); + CloseHandle(Cport[comport_number]); + return(1); + } + + COMMTIMEOUTS Cptimeouts; + + Cptimeouts.ReadIntervalTimeout = MAXDWORD; + Cptimeouts.ReadTotalTimeoutMultiplier = 0; + Cptimeouts.ReadTotalTimeoutConstant = 0; + Cptimeouts.WriteTotalTimeoutMultiplier = 0; + Cptimeouts.WriteTotalTimeoutConstant = 0; + + if(!SetCommTimeouts(Cport[comport_number], &Cptimeouts)) + { + printf("unable to set comport time-out settings\n"); + CloseHandle(Cport[comport_number]); + return(1); + } + + return(0); +} + + +int RS232_PollComport(int comport_number, unsigned char *buf, int size) +{ + int n; + +/* added the void pointer cast, otherwise gcc will complain about */ +/* "warning: dereferencing type-punned pointer will break strict aliasing rules" */ + + ReadFile(Cport[comport_number], buf, size, (LPDWORD)((void *)&n), NULL); + + return(n); +} + + +int RS232_SendByte(int comport_number, unsigned char byte) +{ + int n; + + WriteFile(Cport[comport_number], &byte, 1, (LPDWORD)((void *)&n), NULL); + + if(n<0) return(1); + + return(0); +} + + +int RS232_SendBuf(int comport_number, unsigned char *buf, int size) +{ + int n; + + if(WriteFile(Cport[comport_number], buf, size, (LPDWORD)((void *)&n), NULL)) + { + return(n); + } + + return(-1); +} + + +void RS232_CloseComport(int comport_number) +{ + CloseHandle(Cport[comport_number]); +} + +/* +http://msdn.microsoft.com/en-us/library/windows/desktop/aa363258%28v=vs.85%29.aspx +*/ + +int RS232_IsDCDEnabled(int comport_number) +{ + int status; + + GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status)); + + if(status&MS_RLSD_ON) return(1); + else return(0); +} + + +int RS232_IsCTSEnabled(int comport_number) +{ + int status; + + GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status)); + + if(status&MS_CTS_ON) return(1); + else return(0); +} + + +int RS232_IsDSREnabled(int comport_number) +{ + int status; + + GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status)); + + if(status&MS_DSR_ON) return(1); + else return(0); +} + + +void RS232_enableDTR(int comport_number) +{ + EscapeCommFunction(Cport[comport_number], SETDTR); +} + + +void RS232_disableDTR(int comport_number) +{ + EscapeCommFunction(Cport[comport_number], CLRDTR); +} + + +void RS232_enableRTS(int comport_number) +{ + EscapeCommFunction(Cport[comport_number], SETRTS); +} + + +void RS232_disableRTS(int comport_number) +{ + EscapeCommFunction(Cport[comport_number], CLRRTS); +} + +/* +https://msdn.microsoft.com/en-us/library/windows/desktop/aa363428%28v=vs.85%29.aspx +*/ + +void RS232_flushRX(int comport_number) +{ + PurgeComm(Cport[comport_number], PURGE_RXCLEAR | PURGE_RXABORT); +} + + +void RS232_flushTX(int comport_number) +{ + PurgeComm(Cport[comport_number], PURGE_TXCLEAR | PURGE_TXABORT); +} + + +void RS232_flushRXTX(int comport_number) +{ + PurgeComm(Cport[comport_number], PURGE_RXCLEAR | PURGE_RXABORT); + PurgeComm(Cport[comport_number], PURGE_TXCLEAR | PURGE_TXABORT); +} + + +#endif + + +void RS232_cputs(int comport_number, const char *text) /* sends a string to serial port */ +{ + while(*text != 0) RS232_SendByte(comport_number, *(text++)); +} + + +/* return index in comports matching to device name or -1 if not found */ +int RS232_GetPortnr(const char *devname) +{ + int i; + + char str[32]; + +#if defined(__linux__) || defined(__FreeBSD__) /* Linux & FreeBSD */ + strcpy(str, "/dev/"); +#else /* windows */ + strcpy(str, "\\\\.\\"); +#endif + strncat(str, devname, 16); + str[31] = 0; + + for(i=0; i<RS232_PORTNR; i++) + { + if(!strcmp(comports[i], str)) + { + return i; + } + } + + return -1; /* device not found */ +} + + + + + + + + + + + diff --git a/Modules/Mavlink/rs232.h b/Modules/Mavlink/rs232.h new file mode 100644 index 00000000..eee239f9 --- /dev/null +++ b/Modules/Mavlink/rs232.h @@ -0,0 +1,87 @@ +/* +*************************************************************************** +* +* Author: Teunis van Beelen +* +* Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017 Teunis van Beelen +* +* Email: teuniz@gmail.com +* +*************************************************************************** +* +* This program 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. +* +* This program 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 this program. If not, see <http://www.gnu.org/licenses/>. +* +*************************************************************************** +*/ + +/* Last revision: August 5, 2017 */ + +/* For more info and how to use this library, visit: http://www.teuniz.net/RS-232/ */ + + +#ifndef rs232_INCLUDED +#define rs232_INCLUDED + +#ifdef __cplusplus +extern "C" { +#endif + +#include <stdio.h> +#include <string.h> + + + +#if defined(__linux__) || defined(__FreeBSD__) || defined(__APPLE__) + +#include <termios.h> +#include <sys/ioctl.h> +#include <unistd.h> +#include <fcntl.h> +#include <sys/types.h> +#include <sys/stat.h> +#include <limits.h> +#include <sys/file.h> +#include <errno.h> + +#else + +#include <windows.h> + +#endif + +int RS232_OpenComport(int, int, const char *); +int RS232_PollComport(int, unsigned char *, int); +int RS232_SendByte(int, unsigned char); +int RS232_SendBuf(int, unsigned char *, int); +void RS232_CloseComport(int); +void RS232_cputs(int, const char *); +int RS232_IsDCDEnabled(int); +int RS232_IsCTSEnabled(int); +int RS232_IsDSREnabled(int); +void RS232_enableDTR(int); +void RS232_disableDTR(int); +void RS232_enableRTS(int); +void RS232_disableRTS(int); +void RS232_flushRX(int); +void RS232_flushTX(int); +void RS232_flushRXTX(int); +int RS232_GetPortnr(const char *); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif + + diff --git a/Modules/skywardhubstrings.cpp b/Modules/skywardhubstrings.cpp index 7f6e8e63..8099e019 100644 --- a/Modules/skywardhubstrings.cpp +++ b/Modules/skywardhubstrings.cpp @@ -5,6 +5,7 @@ const QString SkywardHubStrings::defaultConfigurationFolder = "SkywardHubConfig"; const QString SkywardHubStrings::defaultSettingsFilePath = defaultConfigurationFolder + "/" + "settings.xml"; const QString SkywardHubStrings::defaultPrefabsFolderName = "Prefabs"; +const QString SkywardHubStrings::defaultLogsFolder = defaultConfigurationFolder + "/" + "Logs"; const QString SkywardHubStrings::defaultPrefabsFolder = defaultConfigurationFolder + "/" + defaultPrefabsFolderName +"/"; const QString SkywardHubStrings::defaultConfigurationFileName = "default.xml"; const QString SkywardHubStrings::defaultConfigurationIconPath = defaultConfigurationFolder + "/" + "defaultConfigIcon.svg"; @@ -38,6 +39,7 @@ const QString SkywardHubStrings::mavlink_orientation_tc_roll_name = "roll"; const QString SkywardHubStrings::msgIdField = "id"; const QString SkywardHubStrings::msgSequenceNumberField = "sequenceNumber"; const QString SkywardHubStrings::mavlink_quality_link_topic = "MavlinkLinkQuality"; +const QString SkywardHubStrings::mavlink_received_msg_topic = "MavlinkMessages"; SkywardHubStrings::SkywardHubStrings() diff --git a/Modules/skywardhubstrings.h b/Modules/skywardhubstrings.h index 6d6271ea..a631c0f4 100644 --- a/Modules/skywardhubstrings.h +++ b/Modules/skywardhubstrings.h @@ -14,6 +14,7 @@ public: static const QString defaultPrefabsFolder; static const QString defaultConfigurationFileName; static const QString defaultConfigurationIconPath; + static const QString defaultLogsFolder; // Xml Tags Name static const QString settingsObjectName; @@ -46,6 +47,7 @@ public: static const QString msgSequenceNumberField; static const QString mavlink_quality_link_topic; + static const QString mavlink_received_msg_topic; }; diff --git a/SkywardHub.pro b/SkywardHub.pro index 940a3720..2fb40d62 100644 --- a/SkywardHub.pro +++ b/SkywardHub.pro @@ -47,6 +47,7 @@ SOURCES += \ Modules/Mavlink/mavlinkreader.cpp \ Modules/Mavlink/mavlinkrocketmsgtestingmodule.cpp \ Modules/Mavlink/mavlinkwriter.cpp \ + Modules/Mavlink/rs232.c \ Modules/MessageViewer/messagesviewermodule.cpp \ Modules/ScrollArea/scrollareamodule.cpp \ Modules/SerialStream/serialstreammodule.cpp \ @@ -111,6 +112,7 @@ HEADERS += \ Modules/Mavlink/mavlinkversionheader.h \ Modules/Mavlink/mavlinkversionheader.h \ Modules/Mavlink/mavlinkwriter.h \ + Modules/Mavlink/rs232.h \ Modules/MessageViewer/messagesviewermodule.h \ Modules/ScrollArea/scrollareamodule.h \ Modules/SerialStream/serialstreammodule.h \ diff --git a/SkywardHub.pro.user b/SkywardHub.pro.user index 774f237a..29cabb50 100644 --- a/SkywardHub.pro.user +++ b/SkywardHub.pro.user @@ -1,6 +1,6 @@ <?xml version="1.0" encoding="UTF-8"?> <!DOCTYPE QtCreatorProject> -<!-- Written by QtCreator 4.14.2, 2021-05-26T19:42:19. --> +<!-- Written by QtCreator 4.14.2, 2021-07-14T12:55:11. --> <qtcreator> <data> <variable>EnvironmentId</variable> -- GitLab