From 8c900f05b864e9617b3d34af28ca43afcd13413e Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Fri, 1 Sep 2023 14:31:36 +0000
Subject: [PATCH] [mavlink] Refactored MavlinkModule, added UDP support and
system ID support
---
CMakeLists.txt | 11 +-
SkywardHub.pro | 24 +-
scripts/udp_gs_tester.py | 144 +++++++
.../Modules/Mavlink/BaseMavlinkModule.cpp | 239 +++++++++++
.../{MavlinkModule.h => BaseMavlinkModule.h} | 70 ++--
src/shared/Modules/Mavlink/MavlinkCodec.cpp | 396 ++++++++++++++++++
.../{MavlinkReader.h => MavlinkCodec.h} | 50 ++-
.../Modules/Mavlink/MavlinkCommandAdapter.cpp | 221 ----------
.../Modules/Mavlink/MavlinkCommandAdapter.h | 57 ---
src/shared/Modules/Mavlink/MavlinkModule.cpp | 267 ------------
src/shared/Modules/Mavlink/MavlinkReader.cpp | 243 -----------
.../Modules/Mavlink/Ports/MavlinkPort.cpp | 21 +
.../Modules/Mavlink/Ports/MavlinkPort.h | 35 ++
.../Modules/Mavlink/Ports/SerialPort.cpp | 58 +++
.../{MavlinkWriter.h => Ports/SerialPort.h} | 22 +-
src/shared/Modules/Mavlink/Ports/UdpPort.cpp | 52 +++
.../{MavlinkWriter.cpp => Ports/UdpPort.h} | 39 +-
.../Modules/Mavlink/SerialMavlinkModule.cpp | 117 ++++++
.../Modules/Mavlink/SerialMavlinkModule.h | 49 +++
.../Modules/Mavlink/UdpMavlinkModule.cpp | 98 +++++
src/shared/Modules/Mavlink/UdpMavlinkModule.h | 43 ++
src/shared/Modules/ModuleInfo.h | 3 +-
src/shared/Modules/ModulesList.cpp | 16 +-
23 files changed, 1387 insertions(+), 888 deletions(-)
create mode 100755 scripts/udp_gs_tester.py
create mode 100644 src/shared/Modules/Mavlink/BaseMavlinkModule.cpp
rename src/shared/Modules/Mavlink/{MavlinkModule.h => BaseMavlinkModule.h} (59%)
create mode 100644 src/shared/Modules/Mavlink/MavlinkCodec.cpp
rename src/shared/Modules/Mavlink/{MavlinkReader.h => MavlinkCodec.h} (63%)
delete mode 100644 src/shared/Modules/Mavlink/MavlinkCommandAdapter.cpp
delete mode 100644 src/shared/Modules/Mavlink/MavlinkCommandAdapter.h
delete mode 100644 src/shared/Modules/Mavlink/MavlinkModule.cpp
delete mode 100644 src/shared/Modules/Mavlink/MavlinkReader.cpp
create mode 100644 src/shared/Modules/Mavlink/Ports/MavlinkPort.cpp
create mode 100644 src/shared/Modules/Mavlink/Ports/MavlinkPort.h
create mode 100644 src/shared/Modules/Mavlink/Ports/SerialPort.cpp
rename src/shared/Modules/Mavlink/{MavlinkWriter.h => Ports/SerialPort.h} (69%)
create mode 100644 src/shared/Modules/Mavlink/Ports/UdpPort.cpp
rename src/shared/Modules/Mavlink/{MavlinkWriter.cpp => Ports/UdpPort.h} (53%)
create mode 100644 src/shared/Modules/Mavlink/SerialMavlinkModule.cpp
create mode 100644 src/shared/Modules/Mavlink/SerialMavlinkModule.h
create mode 100644 src/shared/Modules/Mavlink/UdpMavlinkModule.cpp
create mode 100644 src/shared/Modules/Mavlink/UdpMavlinkModule.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 64a4c4a4..eb70d9bc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -70,10 +70,13 @@ add_executable(groundstation
src/shared/Modules/IncomingMessagesViewer/IncomingMessagesViewerModule.cpp
src/shared/Modules/MainWindow/SkywardHubMainWindow.cpp
src/shared/Modules/MainWindow/Window.cpp
- src/shared/Modules/Mavlink/MavlinkCommandAdapter.cpp
- src/shared/Modules/Mavlink/MavlinkModule.cpp
- src/shared/Modules/Mavlink/MavlinkReader.cpp
- src/shared/Modules/Mavlink/MavlinkWriter.cpp
+ src/shared/Modules/Mavlink/Ports/MavlinkPort.cpp
+ src/shared/Modules/Mavlink/Ports/SerialPort.cpp
+ src/shared/Modules/Mavlink/Ports/UdpPort.cpp
+ src/shared/Modules/Mavlink/BaseMavlinkModule.cpp
+ src/shared/Modules/Mavlink/MavlinkCodec.cpp
+ src/shared/Modules/Mavlink/SerialMavlinkModule.cpp
+ src/shared/Modules/Mavlink/UdpMavlinkModule.cpp
src/shared/Modules/OutgoingMessagesViewer/OutgoingMessagesViewerModule.cpp
src/shared/Modules/Splitter/Splitter.cpp
src/shared/Modules/OrientationVisualizer/OrientationVisualizer.cpp
diff --git a/SkywardHub.pro b/SkywardHub.pro
index 125b5ae4..e90dd506 100644
--- a/SkywardHub.pro
+++ b/SkywardHub.pro
@@ -1,6 +1,6 @@
QT += core gui 3dcore 3drender 3dinput 3dlogic 3dextras 3danimation
-greaterThan(QT_MAJOR_VERSION, 4): QT += widgets printsupport serialport
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets printsupport serialport network
CONFIG += c++11
@@ -44,10 +44,13 @@ SOURCES += \
src/shared/Modules/CompactCommandPad/CommandSelector.cpp \
src/shared/Modules/Tabs/TabsModule.cpp \
src/shared/Modules/ModuleInfo.cpp \
- src/shared/Modules/Mavlink/MavlinkWriter.cpp \
- src/shared/Modules/Mavlink/MavlinkReader.cpp \
- src/shared/Modules/Mavlink/MavlinkCommandAdapter.cpp \
- src/shared/Modules/Mavlink/MavlinkModule.cpp \
+ src/shared/Modules/Mavlink/Ports/MavlinkPort.cpp \
+ src/shared/Modules/Mavlink/Ports/SerialPort.cpp \
+ src/shared/Modules/Mavlink/Ports/UdpPort.cpp \
+ src/shared/Modules/Mavlink/BaseMavlinkModule.cpp \
+ src/shared/Modules/Mavlink/MavlinkCodec.cpp \
+ src/shared/Modules/Mavlink/SerialMavlinkModule.cpp \
+ src/shared/Modules/Mavlink/UdpMavlinkModule.cpp \
src/shared/Modules/ValvesViewer/ValvesViewer.cpp \
src/shared/Core/EventHandler/EventHandler.cpp \
src/shared/Core/XmlObject.cpp \
@@ -97,11 +100,14 @@ HEADERS += \
src/shared/Modules/CompactCommandPad/CommandSelector.h \
src/shared/Modules/CompactCommandPad/SendThread.h \
src/shared/Modules/Tabs/TabsModule.h \
- src/shared/Modules/Mavlink/MavlinkReader.h \
- src/shared/Modules/Mavlink/MavlinkWriter.h \
+ src/shared/Modules/Mavlink/Ports/MavlinkPort.h \
+ src/shared/Modules/Mavlink/Ports/SerialPort.h \
+ src/shared/Modules/Mavlink/Ports/UdpPort.h \
src/shared/Modules/Mavlink/MavlinkVersionHeader.h \
- src/shared/Modules/Mavlink/MavlinkModule.h \
- src/shared/Modules/Mavlink/MavlinkCommandAdapter.h \
+ src/shared/Modules/Mavlink/BaseMavlinkModule.h \
+ src/shared/Modules/Mavlink/MavlinkCodec.h \
+ src/shared/Modules/Mavlink/SerialMavlinkModule.h \
+ src/shared/Modules/Mavlink/UdpMavlinkModule.h \
src/shared/Modules/ModulesList.h \
src/shared/Modules/ValvesViewer/ValvesViewer.h \
src/shared/Modules/ValvesViewer/ValvesList.h \
diff --git a/scripts/udp_gs_tester.py b/scripts/udp_gs_tester.py
new file mode 100755
index 00000000..f7b069d7
--- /dev/null
+++ b/scripts/udp_gs_tester.py
@@ -0,0 +1,144 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2021 Skyward Experimental Rocketry
+# Author: Davide Mor
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the 'Software'), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED 'AS IS', WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+#
+# Very simple script to test skywardhub UDP mavlink port
+#
+
+import sys, os
+
+# Setup import paths for mavlink
+sys.path.append(os.path.dirname(__file__) + "/../libs/mavlink-skyward-lib")
+sys.path.append(os.path.dirname(__file__) + "/../libs/mavlink-skyward-lib/mavlink")
+
+from mavlink_lib import *
+
+import threading, socket, datetime, time, queue
+
+RECV_PORT = 42070
+SEND_PORT = 42069
+
+start_time = datetime.datetime.now()
+def get_timestamp():
+ now_time = datetime.datetime.now()
+ return int((now_time - start_time) / datetime.timedelta(microseconds=1))
+
+def fake_data(i):
+ return MAVLink_rocket_flight_tm_message(
+ get_timestamp(), # timestamp
+ 0, # ada_state
+ 0, # fmm_state
+ 0, # dpl_state
+ 0, # abk_state
+ 0, # nas_state
+ 0, # mea_state
+ 0.0, # pressure_ada
+ 0.0, # pressure_digi
+ 0.0, # pressure_static
+ 0.0, # pressure_dpl
+ 0.0, # airspeed_pitot
+ 0.0, # altitude_agl
+ 0.0, # ada_vert_speed
+ 0.0, # mea_mass
+ i * 2, # acc_x
+ i * 3, # acc_y
+ i * 4, # acc_z
+ 0.0, # gyro_x
+ 0.0, # gyro_y
+ 0.0, # gyro_z
+ 0.0, # mag_x
+ 0.0, # mag_y
+ 0.0, # mag_z
+ 0, # gps_fix
+ 0.0, # gps_lat
+ 0.0, # gps_lon
+ 0.0, # gps_alt
+ 0.0, # abk_angle
+ 0.0, # nas_n
+ 0.0, # nas_e
+ 0.0, # nas_d
+ 0.0, # nas_vn
+ 0.0, # nas_ve
+ 0.0, # nas_vd
+ 0.0, # nas_qx
+ 0.0, # nas_qy
+ 0.0, # nas_qz
+ 0.0, # nas_qw
+ 0.0, # nas_bias_x
+ 0.0, # nas_bias_y
+ 0.0, # nas_bias_z
+ 0, # pin_launch
+ 0, # pin_nosecone
+ 0, # pin_expulsion
+ 0, # cutter_presence
+ 0.0, # battery_voltage
+ 0.0, # current_consumption
+ 0.0, # cam_battery_voltage
+ 0.0, # temperature
+ 0, # logger_error
+ )
+
+def build_ack(msg):
+ return MAVLink_ack_tm_message(msg.get_msgId(), msg.get_seq())
+
+def recv_loop(sock, mavlink, acks):
+ while True:
+ (data, address) = sock.recvfrom(1024)
+
+ msg = mavlink.parse_char(data)
+ if msg is not None:
+ if msg.get_msgId() != MAVLINK_MSG_ID_ACK_TM:
+ acks.append(build_ack(msg))
+
+ print(msg.get_srcSystem(), msg.get_srcComponent(), msg)
+
+def send_loop(sock, mavlink, acks):
+ i = 0
+ while True:
+ while len(acks) > 0:
+ ack = acks.pop(0)
+ sock.sendto(ack.pack(mavlink), ("255.255.255.255", SEND_PORT))
+
+ data = fake_data(i)
+ sock.sendto(data.pack(mavlink), ("255.255.255.255", SEND_PORT))
+
+ time.sleep(1)
+ i += 1
+
+
+def main():
+ acks = []
+ mavlink = MAVLink(None)
+
+ sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ sock.bind(("", RECV_PORT))
+ sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+
+ t1 = threading.Thread(target=lambda: send_loop(sock, mavlink, acks))
+ t2 = threading.Thread(target=lambda: recv_loop(sock, mavlink, acks))
+
+ t1.start()
+ t2.start()
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/shared/Modules/Mavlink/BaseMavlinkModule.cpp b/src/shared/Modules/Mavlink/BaseMavlinkModule.cpp
new file mode 100644
index 00000000..2dc7c52a
--- /dev/null
+++ b/src/shared/Modules/Mavlink/BaseMavlinkModule.cpp
@@ -0,0 +1,239 @@
+/*
+ * This file is part of Skyward Hub.
+ *
+ * Skyward Hub 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.
+ *
+ * Skyward Hub 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
+ * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "BaseMavlinkModule.h"
+
+#include <Modules/SkywardHubStrings.h>
+
+BaseMavlinkModule::BaseMavlinkModule(MavlinkPort *port, QWidget *parent)
+ : DefaultModule(parent), port(port), mavlinkCodec(port, this)
+{
+ setupUi();
+ defaultContextMenuSetup();
+
+ connect(&mavlinkCodec, &MavlinkCodec::msgReceived, this,
+ &BaseMavlinkModule::onMsgReceived);
+ connect(&linkQualityTimer, &QTimer::timeout, this,
+ &BaseMavlinkModule::onLinkQualityTimerTick);
+
+ getCore()->getMessageBroker()->subscribe(
+ Filter::fromString(SkywardHubStrings::commandsTopic + "/*"), this,
+ [this](const Message &message, const Filter &filter)
+ { onCommandReceived(message); });
+}
+
+BaseMavlinkModule::~BaseMavlinkModule()
+{
+ if (startToggleButton->state())
+ stop();
+
+ getCore()->getMessageBroker()->unsubscribe(
+ Filter::fromString(SkywardHubStrings::commandsTopic + "/*"), this);
+}
+
+QWidget *BaseMavlinkModule::toWidget() { return this; }
+
+XmlObject BaseMavlinkModule::toXmlObject()
+{
+ XmlObject obj = childToXmlObject();
+ obj.addAttribute("log_file", logCheckBox->isChecked() ? 1 : 0);
+
+ return obj;
+}
+
+void BaseMavlinkModule::fromXmlObject(const XmlObject &obj)
+{
+ childFromXmlObject(obj);
+
+ int logFile;
+ obj.getAttribute("log_file", logFile);
+ logCheckBox->setChecked(logFile != 0);
+}
+
+void BaseMavlinkModule::onMsgReceived(const Message &msg)
+{
+ msgArrived++;
+ getCore()->getMessageBroker()->publish(msg);
+}
+
+void BaseMavlinkModule::onLinkQualityTimerTick()
+{
+ float ratio = (float)msgArrived / linkQualityPeriod * 1000.0;
+ msgArrived = 0;
+ rateLabel->setText("Rate: " + QString::number(ratio) + " msg/s");
+}
+
+void BaseMavlinkModule::onStartStreamToggled(bool state)
+{
+ if (state)
+ onStartClicked();
+ else
+ onStopClicked();
+}
+
+void BaseMavlinkModule::onStartClicked()
+{
+ if (open())
+ {
+ disableControls();
+ linkQualityTimer.start(linkQualityPeriod);
+
+ // TODO: Allow the checkbox to start and stop the logging
+ if (logCheckBox->isChecked())
+ mavlinkCodec.openLogFile();
+
+ mavlinkCodec.startReading();
+ }
+ else
+ {
+ error(tr("Mavlink"), tr("Unable to open selected port."));
+ startToggleButton->setState(false);
+ }
+}
+
+void BaseMavlinkModule::onStopClicked()
+{
+ stop();
+ port->close();
+ enableControls();
+}
+
+void BaseMavlinkModule::onCommandReceived(const Message &msg)
+{
+ MavlinkCodec::SysId sysId =
+ static_cast<MavlinkCodec::SysId>(sysIdComboBox->currentData().toInt());
+
+ mavlink_message_t mav_msg;
+ if (!mavlinkCodec.encodeMessage(msg, sysId, MavlinkCodec::CompIdNone,
+ mav_msg))
+ {
+ error(tr("Mavlink"),
+ tr("Cannot encode command into a valid mavlink message."));
+ return;
+ }
+
+ if (!port->isOpen())
+ {
+ error(tr("Mavlink"), tr("Cannot send message: port is closed."));
+ return;
+ }
+
+ mavlinkCodec.send(mav_msg);
+
+ Message confirmationMsg(
+ Topic((QString)SkywardHubStrings::logCommandsTopic));
+
+ auto nameField = Field(msg.getTopic().toString().replace(
+ SkywardHubStrings::commandsTopic + "/", ""));
+ confirmationMsg.setField("name", nameField);
+
+ auto idField = Field((uint64_t)mav_msg.msgid);
+ confirmationMsg.setField("message_id", idField);
+
+ auto seqField = Field((uint64_t)mav_msg.seq);
+ confirmationMsg.setField("sequence_number", seqField);
+
+ getCore()->getMessageBroker()->publish(confirmationMsg);
+}
+
+void BaseMavlinkModule::onOpenLogFolderClick()
+{
+ QDir dir(SkywardHubStrings::defaultLogsFolder);
+ if (dir.exists())
+ {
+ QDesktopServices::openUrl(
+ QUrl::fromLocalFile(SkywardHubStrings::defaultLogsFolder));
+ }
+ else
+ {
+ warning(tr("Mavlink"), tr("The log folder does not exist yet. It will "
+ "be created when opening the serial port."));
+ }
+}
+
+void BaseMavlinkModule::enableControls()
+{
+ sysIdComboBox->setEnabled(true);
+ logCheckBox->setEnabled(true);
+ childEnableControls();
+}
+
+void BaseMavlinkModule::disableControls()
+{
+ sysIdComboBox->setEnabled(false);
+ logCheckBox->setEnabled(false);
+ childDisableControls();
+}
+
+void BaseMavlinkModule::stop()
+{
+ linkQualityTimer.stop();
+ mavlinkCodec.stopReading();
+}
+
+void BaseMavlinkModule::setupUi()
+{
+ QHBoxLayout *outerLayout = new QHBoxLayout;
+ outerLayout->setContentsMargins(6, 0, 6, 0);
+
+ startToggleButton = new ToggleButton;
+ startToggleButton->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+ outerLayout->addWidget(startToggleButton);
+ connect(startToggleButton, &ToggleButton::toggled, this,
+ &BaseMavlinkModule::onStartStreamToggled);
+
+ QLabel *sysIdLabel = new QLabel("Target system:");
+ sysIdLabel->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+ outerLayout->addWidget(sysIdLabel);
+
+ sysIdComboBox = new QComboBox;
+ sysIdComboBox->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+
+ sysIdComboBox->addItem("Main", MavlinkCodec::SysIdMain);
+ sysIdComboBox->addItem("Payload", MavlinkCodec::SysIdPayload);
+ sysIdComboBox->addItem("Rig", MavlinkCodec::SysIdRig);
+ sysIdComboBox->addItem("Gs Receiver", MavlinkCodec::SysIdGsReceiver);
+ outerLayout->addWidget(sysIdComboBox);
+
+ childLayout = new QHBoxLayout;
+ outerLayout->addLayout(childLayout);
+
+ outerLayout->addStretch();
+
+ rateLabel = new QLabel("Rate: 0 msg/s");
+ rateLabel->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+ outerLayout->addWidget(rateLabel);
+
+ logCheckBox = new QCheckBox("Enable log");
+ logCheckBox->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+ outerLayout->addWidget(logCheckBox);
+
+ QPushButton *openLogFolderButton = new QPushButton("Log folder");
+ openLogFolderButton->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+ outerLayout->addWidget(openLogFolderButton);
+ connect(openLogFolderButton, &QPushButton::clicked, this,
+ &BaseMavlinkModule::onOpenLogFolderClick);
+
+ setLayout(outerLayout);
+}
\ No newline at end of file
diff --git a/src/shared/Modules/Mavlink/MavlinkModule.h b/src/shared/Modules/Mavlink/BaseMavlinkModule.h
similarity index 59%
rename from src/shared/Modules/Mavlink/MavlinkModule.h
rename to src/shared/Modules/Mavlink/BaseMavlinkModule.h
index 8a4e3693..8438fd31 100644
--- a/src/shared/Modules/Mavlink/MavlinkModule.h
+++ b/src/shared/Modules/Mavlink/BaseMavlinkModule.h
@@ -18,73 +18,73 @@
#pragma once
+#include <Components/ToggleButton/ToggleButton.h>
+#include <Core/Message/Message.h>
#include <Core/Module/Module.h>
+#include <Modules/DefaultModule/DefaultModule.h>
-#include <QSerialPort>
#include <QTimer>
#include <QWidget>
-#include "Components/ToggleButton/ToggleButton.h"
-#include "Core/Message/Message.h"
-#include "MavlinkCommandAdapter.h"
-#include "MavlinkReader.h"
-#include "Modules/DefaultModule/DefaultModule.h"
+#include "MavlinkCodec.h"
+#include "Ports/MavlinkPort.h"
Q_DECLARE_METATYPE(Message);
namespace Ui
{
-class MavlinkModule;
+class BaseMavlinkModule;
}
-class MavlinkModule : public DefaultModule
+class BaseMavlinkModule : public DefaultModule
{
Q_OBJECT
-
public:
- explicit MavlinkModule(QWidget *parent = nullptr);
- ~MavlinkModule();
+ BaseMavlinkModule(MavlinkPort *port, QWidget *parent = nullptr);
+ ~BaseMavlinkModule();
QWidget *toWidget() override;
+
XmlObject toXmlObject() override;
- void fromXmlObject(const XmlObject &xmlObject) override;
+ void fromXmlObject(const XmlObject &obj) override;
- void onCommandReceived(const Message &msg);
+protected:
+ virtual void childDisableControls() = 0;
+ virtual void childEnableControls() = 0;
+ virtual XmlObject childToXmlObject() = 0;
+ virtual void childFromXmlObject(const XmlObject &obj) = 0;
+ virtual bool open() = 0;
-public slots:
- void onStartClicked();
- void onStopClicked();
+ QHBoxLayout *childLayout;
+private slots:
void onMsgReceived(const Message &msg);
void onLinkQualityTimerTick();
+ void onStartStreamToggled(bool state);
-protected:
- mavlink_message_t parseMavlinkMsg(char *buffer, int readCount);
-
- void closePort();
- void updateLinkSignalIndicator();
-
+private:
+ void onStartClicked();
+ void onStopClicked();
+ void onCommandReceived(const Message &msg);
void onOpenLogFolderClick();
-private slots:
- void onStartStreamToggled(bool state);
-
private:
+ void enableControls();
+ void disableControls();
+
+ void stop();
+
void setupUi();
- void initializeSerialPortView();
- bool startReadingOnSerialPort();
- ToggleButton startSerialStreamToggleButton;
- MavlinkReader mavlinkReader;
- MavlinkCommandAdapter mavlinkCommandAdapter;
- QSerialPort serialPort;
+ MavlinkPort *port;
+ MavlinkCodec mavlinkCodec;
+
+ ToggleButton *startToggleButton;
+ QComboBox *sysIdComboBox;
- QComboBox *portsComboBox;
- QComboBox *baudrateComboBox;
QLabel *rateLabel;
QCheckBox *logCheckBox;
QTimer linkQualityTimer;
const int linkQualityPeriod = 2000; // [ms]
int msgArrived = 0;
- bool portOpen;
-};
+};
\ No newline at end of file
diff --git a/src/shared/Modules/Mavlink/MavlinkCodec.cpp b/src/shared/Modules/Mavlink/MavlinkCodec.cpp
new file mode 100644
index 00000000..1e8648bc
--- /dev/null
+++ b/src/shared/Modules/Mavlink/MavlinkCodec.cpp
@@ -0,0 +1,396 @@
+/*
+ * This file is part of Skyward Hub.
+ *
+ * Skyward Hub 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.
+ *
+ * Skyward Hub 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
+ * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "MavlinkCodec.h"
+
+#include <QDir>
+
+#include "BaseMavlinkModule.h"
+#include "Modules/SkywardHubStrings.h"
+
+MavlinkCodec::MavlinkCodec(MavlinkPort* port, BaseMavlinkModule* parent)
+ : port(port), parent(parent)
+{
+}
+
+void MavlinkCodec::send(const mavlink_message_t& msg)
+{
+ unsigned char
+ buf[MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MAX_DIALECT_PAYLOAD_SIZE];
+ uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
+
+ port->writeBytes(reinterpret_cast<char*>(buf), len);
+}
+
+void MavlinkCodec::startReading()
+{
+ QObject::connect(port, &MavlinkPort::bytesReceived, this,
+ &MavlinkCodec::onBytesReceived);
+}
+
+void MavlinkCodec::stopReading()
+{
+ QObject::disconnect(port, &MavlinkPort::bytesReceived, this,
+ &MavlinkCodec::onBytesReceived);
+}
+
+void MavlinkCodec::closeLog()
+{
+ if (logFile.isOpen())
+ logFile.close();
+}
+
+void MavlinkCodec::openLogFile()
+{
+ closeLog();
+
+ setLogFilePath();
+
+ logFile.setFileName(logFilePath);
+ if (logFile.open(QIODevice::WriteOnly))
+ {
+ if (parent != nullptr)
+ {
+ parent->info(tr("Mavlink"), tr("Opened file ") + logFilePath +
+ tr(" for logging."));
+ }
+ QTextStream out(&logFile);
+ out << "Log started " << QDateTime::currentDateTime().toString()
+ << "\n";
+ }
+ else
+ {
+ if (parent != nullptr)
+ {
+ parent->error(tr("Mavlink"),
+ tr("Error, cannot open file ") + logFilePath);
+ }
+ }
+}
+
+const mavlink_message_info_t& MavlinkCodec::getMessageFormat(uint8_t messageId)
+{
+ static const mavlink_message_info_t infos[256] = MAVLINK_MESSAGE_INFO;
+ return infos[messageId];
+}
+
+void MavlinkCodec::onBytesReceived(const char* data, qsizetype len)
+{
+ for (qsizetype i = 0; i < len; i++)
+ {
+ if (mavlink_parse_char(MAVLINK_COMM_0, data[i], &mavlinkMessage,
+ &mavlinkStatus))
+ emit msgReceived(decodeMessage(mavlinkMessage));
+ }
+
+ if (logFile.isOpen())
+ logFile.write(data, len);
+}
+
+void MavlinkCodec::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;
+}
+
+Message MavlinkCodec::decodeMessage(const mavlink_message_t& msg)
+{
+ const mavlink_message_info_t& info = getMessageFormat(msg.msgid);
+
+ QMap<QString, Field> fields;
+ for (unsigned i = 0; i < info.num_fields; i++)
+ fields[QString(info.fields[i].name)] = decodeField(msg, info.fields[i]);
+
+ Message output;
+ output.setTopic(
+ Topic(SkywardHubStrings::mavlink_received_msg_topic + "/" + info.name));
+ output.setFields(std::move(fields));
+ return output;
+}
+
+Field MavlinkCodec::decodeField(const mavlink_message_t& msg,
+ const mavlink_field_info_t& field)
+{
+ if (field.array_length == 0)
+ {
+ return decodeArrayElement(msg, field, 0);
+ }
+ else
+ {
+ if (field.type == MAVLINK_TYPE_CHAR)
+ {
+ QString str;
+
+ for (unsigned i = 0; i < field.array_length; i++)
+ {
+ str.append(_MAV_RETURN_char(&msg, field.wire_offset + i));
+
+ if (_MAV_RETURN_char(&msg, field.wire_offset + i) == '\0')
+ break;
+ }
+
+ return Field(str);
+ }
+ else
+ {
+ return Field();
+ }
+ }
+}
+
+Field MavlinkCodec::decodeArrayElement(const mavlink_message_t& msg,
+ const mavlink_field_info_t& field,
+ int idx)
+{
+ switch (field.type)
+ {
+ case MAVLINK_TYPE_CHAR:
+ {
+ return Field(static_cast<uint64_t>(
+ _MAV_RETURN_char(&msg, field.wire_offset + idx * 1)));
+ }
+ case MAVLINK_TYPE_UINT8_T:
+ {
+ return Field(static_cast<uint64_t>(
+ _MAV_RETURN_uint8_t(&msg, field.wire_offset + idx * 1)));
+ }
+ case MAVLINK_TYPE_INT8_T:
+ {
+ return Field(static_cast<int64_t>(
+ _MAV_RETURN_int8_t(&msg, field.wire_offset + idx * 1)));
+ }
+ case MAVLINK_TYPE_UINT16_T:
+ {
+ return Field(static_cast<uint64_t>(
+ _MAV_RETURN_uint16_t(&msg, field.wire_offset + idx * 2)));
+ }
+ case MAVLINK_TYPE_INT16_T:
+ {
+ return Field(static_cast<int64_t>(
+ _MAV_RETURN_int16_t(&msg, field.wire_offset + idx * 2)));
+ }
+ case MAVLINK_TYPE_UINT32_T:
+ {
+ return Field(static_cast<uint64_t>(
+ _MAV_RETURN_uint32_t(&msg, field.wire_offset + idx * 4)));
+ }
+ case MAVLINK_TYPE_INT32_T:
+ {
+ return Field(static_cast<int64_t>(
+ _MAV_RETURN_int32_t(&msg, field.wire_offset + idx * 4)));
+ }
+ case MAVLINK_TYPE_UINT64_T:
+ {
+ return Field(static_cast<uint64_t>(
+ _MAV_RETURN_uint64_t(&msg, field.wire_offset + idx * 8)));
+ }
+ case MAVLINK_TYPE_INT64_T:
+ {
+ return Field(static_cast<int64_t>(
+ _MAV_RETURN_int64_t(&msg, field.wire_offset + idx * 8)));
+ }
+ case MAVLINK_TYPE_FLOAT:
+ {
+ return Field(static_cast<double>(
+ _MAV_RETURN_float(&msg, field.wire_offset + idx * 4)));
+ }
+ case MAVLINK_TYPE_DOUBLE:
+ {
+ return Field(static_cast<double>(
+ _MAV_RETURN_double(&msg, field.wire_offset + idx * 8)));
+ }
+ default:
+ {
+ // Unsupported: return EMPTY
+ return Field();
+ }
+ }
+}
+
+bool MavlinkCodec::encodeMessage(const Message& msg, SysId sysId, CompId compId,
+ mavlink_message_t& output)
+{
+
+ QString messageName = msg.getTopic().toString().replace(
+ SkywardHubStrings::commandsTopic + "/", "");
+ if (messageName == "PING_TC")
+ {
+ mavlink_msg_ping_tc_pack(
+ sysId, compId, &output,
+ msg.getField("timestamp").getUnsignedInteger());
+ return true;
+ }
+ else if (messageName == "COMMAND_TC")
+ {
+ mavlink_msg_command_tc_pack(
+ sysId, compId, &output,
+ msg.getField("command_id").getUnsignedInteger());
+ return true;
+ }
+ else if (messageName == "SYSTEM_TM_REQUEST_TC")
+ {
+ mavlink_msg_system_tm_request_tc_pack(
+ sysId, compId, &output, msg.getField("tm_id").getUnsignedInteger());
+ return true;
+ }
+ else if (messageName == "SENSOR_TM_REQUEST_TC")
+ {
+ mavlink_msg_sensor_tm_request_tc_pack(
+ sysId, compId, &output,
+ msg.getField("sensor_id").getUnsignedInteger());
+ return true;
+ }
+ else if (messageName == "SERVO_TM_REQUEST_TC")
+ {
+ mavlink_msg_servo_tm_request_tc_pack(
+ sysId, compId, &output,
+ msg.getField("servo_id").getUnsignedInteger());
+ return true;
+ }
+ else if (messageName == "SET_SERVO_ANGLE_TC")
+ {
+ mavlink_msg_set_servo_angle_tc_pack(
+ sysId, compId, &output,
+ msg.getField("servo_id").getUnsignedInteger(),
+ msg.getField("angle").getDouble());
+ return true;
+ }
+ else if (messageName == "WIGGLE_SERVO_TC")
+ {
+ mavlink_msg_wiggle_servo_tc_pack(
+ sysId, compId, &output,
+ msg.getField("servo_id").getUnsignedInteger());
+ return true;
+ }
+ else if (messageName == "RESET_SERVO_TC")
+ {
+ mavlink_msg_reset_servo_tc_pack(
+ sysId, compId, &output,
+ msg.getField("servo_id").getUnsignedInteger());
+ return true;
+ }
+ else if (messageName == "SET_REFERENCE_ALTITUDE_TC")
+ {
+ mavlink_msg_set_reference_altitude_tc_pack(
+ sysId, compId, &output, msg.getField("ref_altitude").getDouble());
+ return true;
+ }
+ else if (messageName == "SET_REFERENCE_TEMPERATURE_TC")
+ {
+ mavlink_msg_set_reference_temperature_tc_pack(
+ sysId, compId, &output, msg.getField("ref_temp").getDouble());
+ return true;
+ }
+ else if (messageName == "SET_DEPLOYMENT_ALTITUDE_TC")
+ {
+ mavlink_msg_set_deployment_altitude_tc_pack(
+ sysId, compId, &output, msg.getField("dpl_altitude").getDouble());
+ return true;
+ }
+ else if (messageName == "SET_ORIENTATION_TC")
+ {
+ mavlink_msg_set_orientation_tc_pack(sysId, compId, &output,
+ msg.getField("yaw").getDouble(),
+ msg.getField("pitch").getDouble(),
+ msg.getField("roll").getDouble());
+ return true;
+ }
+ else if (messageName == "SET_COORDINATES_TC")
+ {
+ mavlink_msg_set_coordinates_tc_pack(
+ sysId, compId, &output, msg.getField("latitude").getDouble(),
+ msg.getField("longitude").getDouble());
+ return true;
+ }
+ else if (messageName == "RAW_EVENT_TC")
+ {
+ mavlink_msg_raw_event_tc_pack(
+ sysId, compId, &output,
+ msg.getField("topic_id").getUnsignedInteger(),
+ msg.getField("event_id").getUnsignedInteger());
+ return true;
+ }
+ else if (messageName == "SET_TARGET_COORDINATES_TC")
+ {
+ mavlink_msg_set_target_coordinates_tc_pack(
+ sysId, compId, &output, msg.getField("latitude").getDouble(),
+ msg.getField("longitude").getDouble());
+ return true;
+ }
+ else if (messageName == "SET_ALGORITHM_TC")
+ {
+ mavlink_msg_set_algorithm_tc_pack(
+ sysId, compId, &output,
+ msg.getField("algorithm_number").getUnsignedInteger());
+ return true;
+ }
+ else if (messageName == "SET_ATOMIC_VALVE_TIMING_TC")
+ {
+ mavlink_msg_set_atomic_valve_timing_tc_pack(
+ sysId, compId, &output,
+ msg.getField("servo_id").getUnsignedInteger(),
+ msg.getField("maximum_timing").getUnsignedInteger());
+ return true;
+ }
+ else if (messageName == "SET_VALVE_MAXIMUM_APERTURE_TC")
+ {
+ mavlink_msg_set_valve_maximum_aperture_tc_pack(
+ sysId, compId, &output,
+ msg.getField("servo_id").getUnsignedInteger(),
+ msg.getField("maximum_aperture").getDouble());
+ return true;
+ }
+ else if (messageName == "SET_IGNITION_TIME_TC")
+ {
+ mavlink_msg_set_ignition_time_tc_pack(
+ sysId, compId, &output,
+ msg.getField("timing").getUnsignedInteger());
+ return true;
+ }
+ else if (messageName == "CONRIG_STATE_TC")
+ {
+ mavlink_msg_conrig_state_tc_pack(
+ sysId, compId, &output,
+ msg.getField("ignition_btn").getUnsignedInteger(),
+ msg.getField("filling_valve_btn").getUnsignedInteger(),
+ msg.getField("venting_valve_btn").getUnsignedInteger(),
+ msg.getField("release_pressure_btn").getUnsignedInteger(),
+ msg.getField("quick_connector_btn").getUnsignedInteger(),
+ msg.getField("start_tars_btn").getUnsignedInteger(),
+ msg.getField("arm_switch").getUnsignedInteger());
+ return true;
+ }
+
+ return false;
+}
diff --git a/src/shared/Modules/Mavlink/MavlinkReader.h b/src/shared/Modules/Mavlink/MavlinkCodec.h
similarity index 63%
rename from src/shared/Modules/Mavlink/MavlinkReader.h
rename to src/shared/Modules/Mavlink/MavlinkCodec.h
index 651c0641..2d794d50 100644
--- a/src/shared/Modules/Mavlink/MavlinkReader.h
+++ b/src/shared/Modules/Mavlink/MavlinkCodec.h
@@ -18,29 +18,44 @@
#pragma once
-#include <QDateTime>
+#include <Core/Message/Message.h>
+
#include <QFile>
-#include <QSerialPort>
+#include <QObject>
-#include "Core/Message/Message.h"
-#include "Core/XmlObject.h"
#include "MavlinkVersionHeader.h"
+#include "Ports/MavlinkPort.h"
-class MavlinkModule;
+class BaseMavlinkModule;
-class MavlinkReader : public QObject
+class MavlinkCodec : public QObject
{
Q_OBJECT
public:
- explicit MavlinkReader(MavlinkModule* parent);
- ~MavlinkReader();
+ enum SysId
+ {
+ SysIdMain = MAV_SYSID_MAIN,
+ SysIdPayload = MAV_SYSID_PAYLOAD,
+ SysIdRig = MAV_SYSID_RIG,
+ SysIdGsReceiver = MAV_SYSID_GS,
+ };
+
+ enum CompId
+ {
+ CompIdNone = 0
+ };
+
+ explicit MavlinkCodec(MavlinkPort* port, BaseMavlinkModule* parent);
+
+ void send(const mavlink_message_t& msg);
void startReading();
void stopReading();
- Message generateMessage(const mavlink_message_t& msg);
+ bool encodeMessage(const Message& msg, SysId sysId, CompId compId,
+ mavlink_message_t& output);
+ Message decodeMessage(const mavlink_message_t& msg);
- void setSerialPort(QSerialPort* port);
void closeLog();
void openLogFile();
@@ -50,9 +65,9 @@ signals:
void msgReceived(const Message& msg);
private slots:
- void onReadyRead();
+ void onBytesReceived(const char* data, qsizetype len);
-protected:
+private:
/* convert a field of any type to a string */
Field decodeField(const mavlink_message_t& msg,
const mavlink_field_info_t& field);
@@ -63,13 +78,14 @@ protected:
void setLogFilePath();
-private:
- mavlink_message_t decodedMessage;
+ mavlink_message_t mavlinkMessage;
mavlink_status_t mavlinkStatus;
- QSerialPort* serialPort = nullptr;
- MavlinkModule* module;
+ MavlinkPort* port;
+ BaseMavlinkModule* parent;
QString logFilePath = "";
QFile logFile;
-};
+
+ QMap<QString, int> msgNameIdMap;
+};
\ No newline at end of file
diff --git a/src/shared/Modules/Mavlink/MavlinkCommandAdapter.cpp b/src/shared/Modules/Mavlink/MavlinkCommandAdapter.cpp
deleted file mode 100644
index d7f63b55..00000000
--- a/src/shared/Modules/Mavlink/MavlinkCommandAdapter.cpp
+++ /dev/null
@@ -1,221 +0,0 @@
-/*
- * This file is part of Skyward Hub.
- *
- * Skyward Hub 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.
- *
- * Skyward Hub 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
- * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
- *
- */
-
-#include "MavlinkCommandAdapter.h"
-
-#include <Core/ModulesManager/ModulesManager.h>
-
-#include "Core/XmlObject.h"
-#include "MavlinkModule.h"
-#include "MavlinkVersionHeader.h"
-#include "Modules/SkywardHubStrings.h"
-
-MavlinkCommandAdapter::MavlinkCommandAdapter() {}
-
-void MavlinkCommandAdapter::setSerialPort(QSerialPort *value)
-{
- serial = value;
-}
-
-bool MavlinkCommandAdapter::encodeCommand(const Message &msg,
- mavlink_message_t &output)
-{
- QString messageName = msg.getTopic().toString().replace(
- SkywardHubStrings::commandsTopic + "/", "");
- if (messageName == "PING_TC")
- {
- mavlink_msg_ping_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("timestamp").getUnsignedInteger());
- return true;
- }
- else if (messageName == "COMMAND_TC")
- {
- mavlink_msg_command_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("command_id").getUnsignedInteger());
- return true;
- }
- else if (messageName == "SYSTEM_TM_REQUEST_TC")
- {
- mavlink_msg_system_tm_request_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("tm_id").getUnsignedInteger());
- return true;
- }
- else if (messageName == "SENSOR_TM_REQUEST_TC")
- {
- mavlink_msg_sensor_tm_request_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("sensor_id").getUnsignedInteger());
- return true;
- }
- else if (messageName == "SERVO_TM_REQUEST_TC")
- {
- mavlink_msg_servo_tm_request_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("servo_id").getUnsignedInteger());
- return true;
- }
- else if (messageName == "SET_SERVO_ANGLE_TC")
- {
- mavlink_msg_set_servo_angle_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("servo_id").getUnsignedInteger(),
- msg.getField("angle").getDouble());
- return true;
- }
- else if (messageName == "WIGGLE_SERVO_TC")
- {
- mavlink_msg_wiggle_servo_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("servo_id").getUnsignedInteger());
- return true;
- }
- else if (messageName == "RESET_SERVO_TC")
- {
- mavlink_msg_reset_servo_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("servo_id").getUnsignedInteger());
- return true;
- }
- else if (messageName == "SET_REFERENCE_ALTITUDE_TC")
- {
- mavlink_msg_set_reference_altitude_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("ref_altitude").getDouble());
- return true;
- }
- else if (messageName == "SET_REFERENCE_TEMPERATURE_TC")
- {
- mavlink_msg_set_reference_temperature_tc_pack(
- MAV_SYS, MAV_CMP, &output, msg.getField("ref_temp").getDouble());
- return true;
- }
- else if (messageName == "SET_DEPLOYMENT_ALTITUDE_TC")
- {
- mavlink_msg_set_deployment_altitude_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("dpl_altitude").getDouble());
- return true;
- }
- else if (messageName == "SET_ORIENTATION_TC")
- {
- mavlink_msg_set_orientation_tc_pack(MAV_SYS, MAV_CMP, &output,
- msg.getField("yaw").getDouble(),
- msg.getField("pitch").getDouble(),
- msg.getField("roll").getDouble());
- return true;
- }
- else if (messageName == "SET_COORDINATES_TC")
- {
- mavlink_msg_set_coordinates_tc_pack(
- MAV_SYS, MAV_CMP, &output, msg.getField("latitude").getDouble(),
- msg.getField("longitude").getDouble());
- return true;
- }
- else if (messageName == "RAW_EVENT_TC")
- {
- mavlink_msg_raw_event_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("topic_id").getUnsignedInteger(),
- msg.getField("event_id").getUnsignedInteger());
- return true;
- }
- else if (messageName == "SET_TARGET_COORDINATES_TC")
- {
- mavlink_msg_set_target_coordinates_tc_pack(
- MAV_SYS, MAV_CMP, &output, msg.getField("latitude").getDouble(),
- msg.getField("longitude").getDouble());
- return true;
- }
- else if (messageName == "SET_ALGORITHM_TC")
- {
- mavlink_msg_set_algorithm_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("algorithm_number").getUnsignedInteger());
- return true;
- }
- else if (messageName == "SET_ATOMIC_VALVE_TIMING_TC")
- {
- mavlink_msg_set_atomic_valve_timing_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("servo_id").getUnsignedInteger(),
- msg.getField("maximum_timing").getUnsignedInteger());
- return true;
- }
- else if (messageName == "SET_VALVE_MAXIMUM_APERTURE_TC")
- {
- mavlink_msg_set_valve_maximum_aperture_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("servo_id").getUnsignedInteger(),
- msg.getField("maximum_aperture").getDouble());
- return true;
- }
- else if (messageName == "SET_IGNITION_TIME_TC")
- {
- mavlink_msg_set_ignition_time_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("timing").getUnsignedInteger());
- return true;
- }
- else if (messageName == "CONRIG_STATE_TC")
- {
- mavlink_msg_conrig_state_tc_pack(
- MAV_SYS, MAV_CMP, &output,
- msg.getField("ignition_btn").getUnsignedInteger(),
- msg.getField("filling_valve_btn").getUnsignedInteger(),
- msg.getField("venting_valve_btn").getUnsignedInteger(),
- msg.getField("release_pressure_btn").getUnsignedInteger(),
- msg.getField("quick_connector_btn").getUnsignedInteger(),
- msg.getField("start_tars_btn").getUnsignedInteger(),
- msg.getField("arm_switch").getUnsignedInteger());
- return true;
- }
-
- return false;
-}
-
-void MavlinkCommandAdapter::send(mavlink_message_t msg)
-{
- mavlinkWriter.setSerialPort(serial);
- mavlinkWriter.write(msg);
-}
-
-bool MavlinkCommandAdapter::produceMsgFromXml(const XmlObject &xml,
- mavlink_message_t *msg)
-{
- bool result = false;
- if (xml.getObjectName() == "ACK_TM")
- {
- QString recv_string = xml.getAttribute("recv_msgid");
- QString seq_string = xml.getAttribute("seq_ack");
-
- bool ok1, ok2;
- uint8_t recvId = recv_string.toUInt(&ok1);
- uint8_t seq = seq_string.toUInt(&ok2);
-
- if (ok1 && ok2)
- {
- mavlink_msg_ack_tm_pack(MAV_SYS, MAV_CMP, msg, recvId, seq);
- }
- result = true;
- }
-
- return result;
-}
diff --git a/src/shared/Modules/Mavlink/MavlinkCommandAdapter.h b/src/shared/Modules/Mavlink/MavlinkCommandAdapter.h
deleted file mode 100644
index d01f1d8d..00000000
--- a/src/shared/Modules/Mavlink/MavlinkCommandAdapter.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * This file is part of Skyward Hub.
- *
- * Skyward Hub 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.
- *
- * Skyward Hub 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
- * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
- *
- */
-
-#pragma once
-
-#include <Core/Message/Message.h>
-#include <Core/ModulesManager/ModulesManager.h>
-
-#include <QMap>
-#include <QObject>
-#include <QSerialPort>
-
-#include "MavlinkVersionHeader.h"
-#include "MavlinkWriter.h"
-
-class MavlinkModule;
-
-/*
- * This class translate commands received from the UI in mavlink messages that
- * can be sent.
- */
-class MavlinkCommandAdapter : public QObject
-{
- Q_OBJECT
-
-public:
- MavlinkCommandAdapter();
-
- void send(mavlink_message_t msg);
- void setSerialPort(QSerialPort* value);
- bool encodeCommand(const Message& msg, mavlink_message_t& output);
-
- bool produceMsgFromXml(const XmlObject& xml, mavlink_message_t* msg);
- static const int MAV_CMP = 96;
- static const int MAV_SYS = 171;
-
-private:
- QSerialPort* serial = nullptr;
-
- MavlinkWriter mavlinkWriter;
- QMap<QString, int> msgNameIdMap;
-};
diff --git a/src/shared/Modules/Mavlink/MavlinkModule.cpp b/src/shared/Modules/Mavlink/MavlinkModule.cpp
deleted file mode 100644
index 9d8b8d64..00000000
--- a/src/shared/Modules/Mavlink/MavlinkModule.cpp
+++ /dev/null
@@ -1,267 +0,0 @@
-/*
- * This file is part of Skyward Hub.
- *
- * Skyward Hub 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.
- *
- * Skyward Hub 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
- * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
- *
- */
-
-#include "MavlinkModule.h"
-
-#include <Core/MessageBroker/MessageBroker.h>
-#include <Modules/SkywardHubStrings.h>
-
-#include <QDebug>
-#include <QSerialPortInfo>
-
-MavlinkModule::MavlinkModule(QWidget *parent)
- : DefaultModule(parent), mavlinkReader(this), serialPort(this)
-{
- setupUi();
- defaultContextMenuSetup();
- initializeSerialPortView();
-
- connect(&mavlinkReader, &MavlinkReader::msgReceived, this,
- &MavlinkModule::onMsgReceived);
- connect(&linkQualityTimer, &QTimer::timeout, this,
- &MavlinkModule::onLinkQualityTimerTick);
-
- mavlinkCommandAdapter.setSerialPort(&serialPort);
-
- getCore()->getMessageBroker()->subscribe(
- Filter::fromString(SkywardHubStrings::commandsTopic + "/*"), this,
- [this](const Message &message, const Filter &filter)
- { onCommandReceived(message); });
-}
-
-MavlinkModule::~MavlinkModule() { onStopClicked(); }
-
-QWidget *MavlinkModule::toWidget() { return this; }
-
-XmlObject MavlinkModule::toXmlObject()
-{
- XmlObject obj(getName(ModuleId::MAVLINK));
- obj.addAttribute("serial_port", portsComboBox->currentText());
- obj.addAttribute("baudrate", baudrateComboBox->currentData().toInt());
- obj.addAttribute("log_file", logCheckBox->isChecked() ? 1 : 0);
- return obj;
-}
-
-void MavlinkModule::fromXmlObject(const XmlObject &obj)
-{
- if (obj.getObjectName() == getName(ModuleId::MAVLINK))
- {
- QString serialPort = obj.getAttribute("serial_port");
- int baudrate;
- obj.getAttribute("baudrate", baudrate);
- int logFile;
- obj.getAttribute("log_file", logFile);
-
- portsComboBox->setCurrentText(serialPort);
- baudrateComboBox->setCurrentText(QString::number(baudrate));
- logCheckBox->setChecked(logFile != 0);
- }
-}
-
-void MavlinkModule::closePort()
-{
- if (serialPort.isOpen())
- serialPort.close();
-}
-
-void MavlinkModule::onStartClicked()
-{
- if (startReadingOnSerialPort())
- {
- linkQualityTimer.start(linkQualityPeriod);
-
- mavlinkReader.setSerialPort(&serialPort);
- mavlinkCommandAdapter.setSerialPort(&serialPort);
-
- // TODO: Allow the checkbox to start and stop the logging
- if (logCheckBox->isChecked())
- mavlinkReader.openLogFile();
-
- mavlinkReader.startReading();
- }
- else
- {
- error("Mavlink", "Unable to open selected serial port.");
- startSerialStreamToggleButton.setState(false);
- }
-}
-
-void MavlinkModule::onStopClicked()
-{
- linkQualityTimer.stop();
- mavlinkReader.stopReading();
- closePort();
-}
-
-void MavlinkModule::onMsgReceived(const Message &msg)
-{
- msgArrived++;
- getCore()->getMessageBroker()->publish(msg);
-}
-
-void MavlinkModule::onStartStreamToggled(bool state)
-{
- if (state)
- onStartClicked();
- else
- onStopClicked();
-}
-
-void MavlinkModule::onLinkQualityTimerTick()
-{
- float ratio = (float)msgArrived / linkQualityPeriod * 1000.0;
- msgArrived = 0;
- rateLabel->setText("Rate: " + QString::number(ratio) + " msg/s");
-}
-
-void MavlinkModule::onOpenLogFolderClick()
-{
- QDir dir(SkywardHubStrings::defaultLogsFolder);
- if (dir.exists())
- {
- QDesktopServices::openUrl(
- QUrl::fromLocalFile(SkywardHubStrings::defaultLogsFolder));
- }
- else
- {
- warning(tr("Mavlink"), tr("The log folder does not exist yet. It will "
- "be created when opening the serial port."));
- }
-}
-
-void MavlinkModule::onCommandReceived(const Message &msg)
-{
- mavlink_message_t encoded_mvl_msg;
- if (!mavlinkCommandAdapter.encodeCommand(msg, encoded_mvl_msg))
- {
- error(tr("Mavlink"),
- tr("Cannot encode command into a valid mavlink message."));
- }
- else if (!serialPort.isOpen())
- {
- error(tr("Mavlink"), "Cannot send message: serial port is closed.");
- }
- else
- {
- mavlinkCommandAdapter.send(encoded_mvl_msg);
-
- Message confirmationMsg(
- Topic((QString)SkywardHubStrings::logCommandsTopic));
-
- auto nameField = Field(msg.getTopic().toString().replace(
- SkywardHubStrings::commandsTopic + "/", ""));
- confirmationMsg.setField("name", nameField);
-
- auto idField = Field((uint64_t)encoded_mvl_msg.msgid);
- confirmationMsg.setField("message_id", idField);
-
- auto seqField = Field((uint64_t)encoded_mvl_msg.seq);
- confirmationMsg.setField("sequence_number", seqField);
-
- getCore()->getMessageBroker()->publish(confirmationMsg);
- }
-}
-
-void MavlinkModule::setupUi()
-{
- QHBoxLayout *outerLayout = new QHBoxLayout;
- outerLayout->setContentsMargins(6, 0, 6, 0);
-
- QLabel *portsLabel = new QLabel("Available ports:");
- portsLabel->setSizePolicy(
- QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
- outerLayout->addWidget(portsLabel);
-
- portsComboBox = new QComboBox;
- portsComboBox->setSizePolicy(
- QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
- outerLayout->addWidget(portsComboBox);
-
- QLabel *baudrateLabel = new QLabel("Baudrate:");
- baudrateLabel->setSizePolicy(
- QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
- outerLayout->addWidget(baudrateLabel);
-
- baudrateComboBox = new QComboBox;
- baudrateComboBox->setSizePolicy(
- QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
- outerLayout->addWidget(baudrateComboBox);
-
- ToggleButton *startToggleButton = new ToggleButton;
- startToggleButton->setSizePolicy(
- QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
- outerLayout->addWidget(startToggleButton);
- connect(startToggleButton, &ToggleButton::toggled, this,
- &MavlinkModule::onStartStreamToggled);
-
- outerLayout->addStretch();
-
- rateLabel = new QLabel("Rate: 0 msg/s");
- rateLabel->setSizePolicy(
- QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
- outerLayout->addWidget(rateLabel);
-
- logCheckBox = new QCheckBox("Enable log");
- logCheckBox->setSizePolicy(
- QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
- outerLayout->addWidget(logCheckBox);
-
- QPushButton *openLogFolderButton = new QPushButton("Log folder");
- openLogFolderButton->setSizePolicy(
- QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
- outerLayout->addWidget(openLogFolderButton);
- connect(openLogFolderButton, &QPushButton::clicked, this,
- &MavlinkModule::onOpenLogFolderClick);
-
- setLayout(outerLayout);
-}
-
-void MavlinkModule::initializeSerialPortView()
-{
- const auto serialPortInfos = QSerialPortInfo::availablePorts();
- for (auto serialPortInfo : serialPortInfos)
- portsComboBox->addItem(serialPortInfo.portName());
-
- baudrateComboBox->addItem("115200", QSerialPort::Baud115200);
- baudrateComboBox->addItem("19200", QSerialPort::Baud19200);
- baudrateComboBox->addItem("9600", QSerialPort::Baud9600);
-}
-
-bool MavlinkModule::startReadingOnSerialPort()
-{
- QString portName = portsComboBox->currentText();
- bool baudrateOk;
- int baudRate = baudrateComboBox->currentData().toInt(&baudrateOk);
-
- // Check if the parameters are ok
- if (!baudrateOk)
- return false;
-
- // The serial port should not be already open
- if (serialPort.isOpen())
- return true;
-
- // Open the serial port
- 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);
-}
diff --git a/src/shared/Modules/Mavlink/MavlinkReader.cpp b/src/shared/Modules/Mavlink/MavlinkReader.cpp
deleted file mode 100644
index fb84f9f5..00000000
--- a/src/shared/Modules/Mavlink/MavlinkReader.cpp
+++ /dev/null
@@ -1,243 +0,0 @@
-/*
- * This file is part of Skyward Hub.
- *
- * Skyward Hub 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.
- *
- * Skyward Hub 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
- * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
- *
- */
-
-#include "MavlinkReader.h"
-
-#include <QDebug>
-#include <QDir>
-#include <QFile>
-#include <QTextStream>
-
-#include "MavlinkCommandAdapter.h"
-#include "MavlinkModule.h"
-#include "Modules/SkywardHubStrings.h"
-
-MavlinkReader::MavlinkReader(MavlinkModule* parent) : module(parent) {}
-
-MavlinkReader::~MavlinkReader() { closeLog(); }
-
-void MavlinkReader::startReading()
-{
- if (serialPort)
- {
- QObject::connect(serialPort, &QSerialPort::readyRead, this,
- &MavlinkReader::onReadyRead);
- }
-}
-
-void MavlinkReader::stopReading()
-{
- if (serialPort)
- {
- QObject::disconnect(serialPort, &QSerialPort::readyRead, this,
- &MavlinkReader::onReadyRead);
- }
-}
-
-const mavlink_message_info_t& MavlinkReader::getMessageFormat(uint8_t messageId)
-{
- static const mavlink_message_info_t infos[256] = MAVLINK_MESSAGE_INFO;
- return infos[messageId];
-}
-
-void MavlinkReader::onReadyRead()
-{
- if (serialPort == nullptr)
- return;
-
- auto bytes = serialPort->readAll();
- for (auto it = bytes.begin(); it != bytes.end(); ++it)
- if (mavlink_parse_char(MAVLINK_COMM_0, *it, &decodedMessage,
- &mavlinkStatus))
- emit msgReceived(generateMessage(decodedMessage));
-
- if (logFile.isOpen())
- logFile.write(bytes.constData(), sizeof(char) * bytes.length());
-}
-
-void MavlinkReader::setSerialPort(QSerialPort* port) { serialPort = port; }
-
-Message MavlinkReader::generateMessage(const mavlink_message_t& msg)
-{
- const mavlink_message_info_t& info = getMessageFormat(msg.msgid);
-
- QMap<QString, Field> fields;
- for (unsigned i = 0; i < info.num_fields; i++)
- fields[QString(info.fields[i].name)] = decodeField(msg, info.fields[i]);
-
- Message output;
- output.setTopic(
- Topic(SkywardHubStrings::mavlink_received_msg_topic + "/" + info.name));
- output.setFields(std::move(fields));
- return output;
-}
-
-void MavlinkReader::closeLog()
-{
- if (logFile.isOpen())
- logFile.close();
-}
-
-void MavlinkReader::openLogFile()
-{
- closeLog();
-
- setLogFilePath();
-
- logFile.setFileName(logFilePath);
- if (logFile.open(QIODevice::WriteOnly))
- {
- if (module != nullptr)
- {
- module->info(tr("Mavlink"), tr("Opened file ") + logFilePath +
- tr(" for logging."));
- }
- QTextStream out(&logFile);
- out << "Log started " << QDateTime::currentDateTime().toString()
- << "\n";
- }
- else
- {
- if (module != nullptr)
- {
- module->error(tr("Mavlink"),
- tr("Error, cannot open file ") + 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;
-}
-
-Field MavlinkReader::decodeField(const mavlink_message_t& msg,
- const mavlink_field_info_t& field)
-{
- if (field.array_length == 0)
- {
- return decodeArrayElement(msg, field, 0);
- }
- else
- {
- if (field.type == MAVLINK_TYPE_CHAR)
- {
- QString str;
-
- for (unsigned i = 0; i < field.array_length; i++)
- {
- str.append(_MAV_RETURN_char(&msg, field.wire_offset + i));
-
- if (_MAV_RETURN_char(&msg, field.wire_offset + i) == '\0')
- break;
- }
-
- return Field(str);
- }
- else
- {
- return Field();
- }
- }
-}
-
-Field MavlinkReader::decodeArrayElement(const mavlink_message_t& msg,
- const mavlink_field_info_t& field,
- int idx)
-{
- switch (field.type)
- {
- case MAVLINK_TYPE_CHAR:
- {
- return Field(static_cast<uint64_t>(
- _MAV_RETURN_char(&msg, field.wire_offset + idx * 1)));
- }
- case MAVLINK_TYPE_UINT8_T:
- {
- return Field(static_cast<uint64_t>(
- _MAV_RETURN_uint8_t(&msg, field.wire_offset + idx * 1)));
- }
- case MAVLINK_TYPE_INT8_T:
- {
- return Field(static_cast<int64_t>(
- _MAV_RETURN_int8_t(&msg, field.wire_offset + idx * 1)));
- }
- case MAVLINK_TYPE_UINT16_T:
- {
- return Field(static_cast<uint64_t>(
- _MAV_RETURN_uint16_t(&msg, field.wire_offset + idx * 2)));
- }
- case MAVLINK_TYPE_INT16_T:
- {
- return Field(static_cast<int64_t>(
- _MAV_RETURN_int16_t(&msg, field.wire_offset + idx * 2)));
- }
- case MAVLINK_TYPE_UINT32_T:
- {
- return Field(static_cast<uint64_t>(
- _MAV_RETURN_uint32_t(&msg, field.wire_offset + idx * 4)));
- }
- case MAVLINK_TYPE_INT32_T:
- {
- return Field(static_cast<int64_t>(
- _MAV_RETURN_int32_t(&msg, field.wire_offset + idx * 4)));
- }
- case MAVLINK_TYPE_UINT64_T:
- {
- return Field(static_cast<uint64_t>(
- _MAV_RETURN_uint64_t(&msg, field.wire_offset + idx * 8)));
- }
- case MAVLINK_TYPE_INT64_T:
- {
- return Field(static_cast<int64_t>(
- _MAV_RETURN_int64_t(&msg, field.wire_offset + idx * 8)));
- }
- case MAVLINK_TYPE_FLOAT:
- {
- return Field(static_cast<double>(
- _MAV_RETURN_float(&msg, field.wire_offset + idx * 4)));
- }
- case MAVLINK_TYPE_DOUBLE:
- {
- return Field(static_cast<double>(
- _MAV_RETURN_double(&msg, field.wire_offset + idx * 8)));
- }
- default:
- {
- // Unsupported: return EMPTY
- return Field();
- }
- }
-}
diff --git a/src/shared/Modules/Mavlink/Ports/MavlinkPort.cpp b/src/shared/Modules/Mavlink/Ports/MavlinkPort.cpp
new file mode 100644
index 00000000..32276142
--- /dev/null
+++ b/src/shared/Modules/Mavlink/Ports/MavlinkPort.cpp
@@ -0,0 +1,21 @@
+/*
+ * This file is part of Skyward Hub.
+ *
+ * Skyward Hub 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.
+ *
+ * Skyward Hub 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
+ * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "MavlinkPort.h"
+
+MavlinkPort::MavlinkPort() {}
\ No newline at end of file
diff --git a/src/shared/Modules/Mavlink/Ports/MavlinkPort.h b/src/shared/Modules/Mavlink/Ports/MavlinkPort.h
new file mode 100644
index 00000000..86dd704e
--- /dev/null
+++ b/src/shared/Modules/Mavlink/Ports/MavlinkPort.h
@@ -0,0 +1,35 @@
+/*
+ * This file is part of Skyward Hub.
+ *
+ * Skyward Hub 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.
+ *
+ * Skyward Hub 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
+ * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include <QObject>
+
+class MavlinkPort : public QObject
+{
+ Q_OBJECT
+public:
+ MavlinkPort();
+
+ virtual void writeBytes(const char *data, qsizetype len) = 0;
+ virtual void close() = 0;
+ virtual bool isOpen() = 0;
+
+signals:
+ void bytesReceived(const char *data, qsizetype len);
+};
\ No newline at end of file
diff --git a/src/shared/Modules/Mavlink/Ports/SerialPort.cpp b/src/shared/Modules/Mavlink/Ports/SerialPort.cpp
new file mode 100644
index 00000000..25163589
--- /dev/null
+++ b/src/shared/Modules/Mavlink/Ports/SerialPort.cpp
@@ -0,0 +1,58 @@
+/*
+ * This file is part of Skyward Hub.
+ *
+ * Skyward Hub 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.
+ *
+ * Skyward Hub 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
+ * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "SerialPort.h"
+
+#include <QSerialPortInfo>
+
+SerialPort::SerialPort(QObject *parent) : serial(parent)
+{
+ QObject::connect(&serial, &QSerialPort::readyRead, this,
+ &SerialPort::onReadyRead);
+}
+
+bool SerialPort::open(QString portName, int baudRate)
+{
+ QSerialPortInfo port(portName);
+ serial.setPort(port);
+ serial.setBaudRate(baudRate);
+ serial.setDataBits(QSerialPort::Data8);
+ serial.setParity(QSerialPort::NoParity);
+ serial.setStopBits(QSerialPort::OneStop);
+
+ if (!serial.open(QIODevice::ReadWrite))
+ return false;
+
+ return true;
+}
+
+void SerialPort::close() { serial.close(); }
+
+bool SerialPort::isOpen() { return serial.isOpen(); }
+
+void SerialPort::writeBytes(const char *data, qsizetype len)
+{
+ serial.write(data, len);
+ serial.flush();
+}
+
+void SerialPort::onReadyRead()
+{
+ auto data = serial.readAll();
+ emit bytesReceived(data.data(), data.size());
+}
\ No newline at end of file
diff --git a/src/shared/Modules/Mavlink/MavlinkWriter.h b/src/shared/Modules/Mavlink/Ports/SerialPort.h
similarity index 69%
rename from src/shared/Modules/Mavlink/MavlinkWriter.h
rename to src/shared/Modules/Mavlink/Ports/SerialPort.h
index 49785244..c721d5b0 100644
--- a/src/shared/Modules/Mavlink/MavlinkWriter.h
+++ b/src/shared/Modules/Mavlink/Ports/SerialPort.h
@@ -18,22 +18,24 @@
#pragma once
-#include <QMutex>
#include <QSerialPort>
-#include "MavlinkVersionHeader.h"
+#include "MavlinkPort.h"
-class MavlinkWriter : public QObject
+class SerialPort : public MavlinkPort
{
Q_OBJECT
-
public:
- MavlinkWriter();
+ SerialPort(QObject *parent = nullptr);
+
+ bool open(QString portName, int baudRate);
+ void close() override;
+ bool isOpen() override;
+ void writeBytes(const char *data, qsizetype len) override;
- void setSerialPort(QSerialPort* port);
- void write(const mavlink_message_t& message);
+private slots:
+ void onReadyRead();
private:
- QSerialPort* serial = nullptr;
- QMutex mtx;
-};
+ QSerialPort serial;
+};
\ No newline at end of file
diff --git a/src/shared/Modules/Mavlink/Ports/UdpPort.cpp b/src/shared/Modules/Mavlink/Ports/UdpPort.cpp
new file mode 100644
index 00000000..082ae059
--- /dev/null
+++ b/src/shared/Modules/Mavlink/Ports/UdpPort.cpp
@@ -0,0 +1,52 @@
+/*
+ * This file is part of Skyward Hub.
+ *
+ * Skyward Hub 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.
+ *
+ * Skyward Hub 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
+ * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "UdpPort.h"
+
+#include <QNetworkDatagram>
+
+UdpPort::UdpPort(QObject *parent) : socket(parent)
+{
+ QObject::connect(&socket, &QUdpSocket::readyRead, this,
+ &UdpPort::onReadyRead);
+}
+
+bool UdpPort::open(int recvPort, int sendPort)
+{
+ this->sendPort = sendPort;
+ return socket.bind(QHostAddress::Null, recvPort);
+}
+
+void UdpPort::close() { socket.close(); }
+
+bool UdpPort::isOpen() { return socket.state() == QUdpSocket::BoundState; }
+
+void UdpPort::writeBytes(const char *data, qsizetype len)
+{
+ socket.writeDatagram(data, len, QHostAddress::Broadcast, sendPort);
+}
+
+void UdpPort::onReadyRead()
+{
+ while (socket.hasPendingDatagrams())
+ {
+ QNetworkDatagram dgram = socket.receiveDatagram();
+ auto data = dgram.data();
+ emit bytesReceived(data.data(), data.size());
+ }
+}
\ No newline at end of file
diff --git a/src/shared/Modules/Mavlink/MavlinkWriter.cpp b/src/shared/Modules/Mavlink/Ports/UdpPort.h
similarity index 53%
rename from src/shared/Modules/Mavlink/MavlinkWriter.cpp
rename to src/shared/Modules/Mavlink/Ports/UdpPort.h
index 00cc6ae5..f74b853b 100644
--- a/src/shared/Modules/Mavlink/MavlinkWriter.cpp
+++ b/src/shared/Modules/Mavlink/Ports/UdpPort.h
@@ -16,26 +16,27 @@
*
*/
-#include "MavlinkWriter.h"
+#pragma once
-#include <chrono>
+#include <QUdpSocket>
-MavlinkWriter::MavlinkWriter() {}
+#include "MavlinkPort.h"
-void MavlinkWriter::write(const mavlink_message_t& message)
+class UdpPort : public MavlinkPort
{
- if (serial == nullptr)
- return;
-
- unsigned char buff[sizeof(mavlink_message_t) + 1];
-
- int msg_len = mavlink_msg_to_send_buffer(buff, &message);
- serial->write(reinterpret_cast<char*>(buff), msg_len);
-
- // if (serial->write(reinterpret_cast<char*>(buff), msg_len) == -1)
- // qDebug() << "MavlinkWriter: Error, writeMsg serial port error";
-
- serial->flush();
-}
-
-void MavlinkWriter::setSerialPort(QSerialPort* port) { serial = port; }
+ Q_OBJECT
+public:
+ UdpPort(QObject *parent = nullptr);
+
+ bool open(int recvPort, int sendPort);
+ void close() override;
+ bool isOpen() override;
+ void writeBytes(const char *data, qsizetype len) override;
+
+private slots:
+ void onReadyRead();
+
+private:
+ int sendPort;
+ QUdpSocket socket;
+};
\ No newline at end of file
diff --git a/src/shared/Modules/Mavlink/SerialMavlinkModule.cpp b/src/shared/Modules/Mavlink/SerialMavlinkModule.cpp
new file mode 100644
index 00000000..45780226
--- /dev/null
+++ b/src/shared/Modules/Mavlink/SerialMavlinkModule.cpp
@@ -0,0 +1,117 @@
+/*
+ * This file is part of Skyward Hub.
+ *
+ * Skyward Hub 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.
+ *
+ * Skyward Hub 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
+ * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "SerialMavlinkModule.h"
+
+#include <QSerialPortInfo>
+
+SerialMavlinkModule::SerialMavlinkModule(QWidget *parent)
+ : BaseMavlinkModule(&serial, parent), serial(this)
+{
+ childSetupUi();
+}
+
+void SerialMavlinkModule::onRefreshClicked() { fillPortsComboBox(); }
+
+XmlObject SerialMavlinkModule::childToXmlObject()
+{
+ XmlObject obj(getName(ModuleId::SERIAL_MAVLINK));
+ obj.addAttribute("serial_port", portsComboBox->currentText());
+ obj.addAttribute("baudrate", baudrateComboBox->currentData().toInt());
+
+ return obj;
+}
+
+void SerialMavlinkModule::childFromXmlObject(const XmlObject &obj)
+{
+ QString serialPort = obj.getAttribute("serial_port");
+ int baudrate;
+ obj.getAttribute("baudrate", baudrate);
+
+ portsComboBox->setCurrentText(serialPort);
+ baudrateComboBox->setCurrentText(QString::number(baudrate));
+}
+
+void SerialMavlinkModule::childSetupUi()
+{
+ QLabel *portsLabel = new QLabel("Available ports:");
+ portsLabel->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+ childLayout->addWidget(portsLabel);
+
+ portsComboBox = new QComboBox;
+ portsComboBox->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+ childLayout->addWidget(portsComboBox);
+
+ fillPortsComboBox();
+
+ refreshButton = new QPushButton("Refresh");
+ refreshButton->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+ connect(refreshButton, &QPushButton::released, this,
+ &SerialMavlinkModule::onRefreshClicked);
+ childLayout->addWidget(refreshButton);
+
+ QLabel *baudrateLabel = new QLabel("Baudrate:");
+ baudrateLabel->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+ childLayout->addWidget(baudrateLabel);
+
+ baudrateComboBox = new QComboBox;
+ baudrateComboBox->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+
+ baudrateComboBox->addItem("115200", QSerialPort::Baud115200);
+ baudrateComboBox->addItem("19200", QSerialPort::Baud19200);
+ baudrateComboBox->addItem("9600", QSerialPort::Baud9600);
+
+ childLayout->addWidget(baudrateComboBox);
+}
+
+void SerialMavlinkModule::childDisableControls()
+{
+ portsComboBox->setEnabled(false);
+ baudrateComboBox->setEnabled(false);
+ refreshButton->setEnabled(false);
+}
+
+void SerialMavlinkModule::childEnableControls()
+{
+ portsComboBox->setEnabled(true);
+ baudrateComboBox->setEnabled(true);
+ refreshButton->setEnabled(true);
+}
+
+bool SerialMavlinkModule::open()
+{
+ QString portName = portsComboBox->currentText();
+ int baudRate = baudrateComboBox->currentData().toInt();
+
+ // Open the serial port
+ return serial.open(portName, baudRate);
+}
+
+void SerialMavlinkModule::fillPortsComboBox()
+{
+ portsComboBox->clear();
+
+ const auto serialPortInfos = QSerialPortInfo::availablePorts();
+ for (auto serialPortInfo : serialPortInfos)
+ portsComboBox->addItem(serialPortInfo.portName());
+}
\ No newline at end of file
diff --git a/src/shared/Modules/Mavlink/SerialMavlinkModule.h b/src/shared/Modules/Mavlink/SerialMavlinkModule.h
new file mode 100644
index 00000000..68a2a7e6
--- /dev/null
+++ b/src/shared/Modules/Mavlink/SerialMavlinkModule.h
@@ -0,0 +1,49 @@
+/*
+ * This file is part of Skyward Hub.
+ *
+ * Skyward Hub 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.
+ *
+ * Skyward Hub 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
+ * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include "BaseMavlinkModule.h"
+#include "Ports/SerialPort.h"
+
+class SerialMavlinkModule : public BaseMavlinkModule
+{
+ Q_OBJECT
+public:
+ explicit SerialMavlinkModule(QWidget *parent = nullptr);
+
+private slots:
+ void onRefreshClicked();
+
+private:
+ void childSetupUi();
+ void childDisableControls() override;
+ void childEnableControls() override;
+
+ XmlObject childToXmlObject() override;
+ void childFromXmlObject(const XmlObject &obj) override;
+ bool open() override;
+
+ void fillPortsComboBox();
+
+ SerialPort serial;
+
+ QComboBox *portsComboBox;
+ QPushButton *refreshButton;
+ QComboBox *baudrateComboBox;
+};
\ No newline at end of file
diff --git a/src/shared/Modules/Mavlink/UdpMavlinkModule.cpp b/src/shared/Modules/Mavlink/UdpMavlinkModule.cpp
new file mode 100644
index 00000000..fd2a60f7
--- /dev/null
+++ b/src/shared/Modules/Mavlink/UdpMavlinkModule.cpp
@@ -0,0 +1,98 @@
+/*
+ * This file is part of Skyward Hub.
+ *
+ * Skyward Hub 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.
+ *
+ * Skyward Hub 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
+ * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "UdpMavlinkModule.h"
+
+UdpMavlinkModule::UdpMavlinkModule(QWidget *parent)
+ : BaseMavlinkModule(&udp, parent), udp(this)
+{
+ childSetupUi();
+}
+
+XmlObject UdpMavlinkModule::childToXmlObject()
+{
+ XmlObject obj(getName(ModuleId::UDP_MAVLINK));
+ obj.addAttribute("recv_port", recvPort->text().toInt());
+ obj.addAttribute("send_port", sendPort->text().toInt());
+
+ return obj;
+}
+
+void UdpMavlinkModule::childFromXmlObject(const XmlObject &obj)
+{
+ QString serialPort = obj.getAttribute("serial_port");
+ int recvPortValue, sendPortValue;
+ obj.getAttribute("recv_port", recvPortValue);
+ obj.getAttribute("send_port", sendPortValue);
+
+ recvPort->setText(QString::number(recvPortValue));
+ sendPort->setText(QString::number(sendPortValue));
+}
+
+void UdpMavlinkModule::childSetupUi()
+{
+ QLabel *recvPortLabel = new QLabel("Receive port:");
+ recvPortLabel->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+ childLayout->addWidget(recvPortLabel);
+
+ recvPort = new QLineEdit;
+ recvPort->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+ recvPort->setValidator(new QIntValidator(0, 0xffff));
+ childLayout->addWidget(recvPort);
+
+ QLabel *sendPortLabel = new QLabel("Send port:");
+ sendPortLabel->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+ childLayout->addWidget(sendPortLabel);
+
+ sendPort = new QLineEdit;
+ sendPort->setSizePolicy(
+ QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+ sendPort->setValidator(new QIntValidator(0, 0xffff));
+ childLayout->addWidget(sendPort);
+}
+
+void UdpMavlinkModule::childDisableControls()
+{
+ recvPort->setEnabled(false);
+ sendPort->setEnabled(false);
+}
+
+void UdpMavlinkModule::childEnableControls()
+{
+ recvPort->setEnabled(true);
+ sendPort->setEnabled(true);
+}
+
+bool UdpMavlinkModule::open()
+{
+ int recvPortValue, sendPortValue;
+ bool recvPortOk, sendPortOk;
+
+ recvPortValue = recvPort->text().toInt(&recvPortOk);
+ sendPortValue = sendPort->text().toInt(&sendPortOk);
+
+ // Check if the parameters are ok
+ if (!recvPortValue || !sendPortValue)
+ return false;
+
+ // Open the serial port
+ return udp.open(recvPortValue, sendPortValue);
+}
\ No newline at end of file
diff --git a/src/shared/Modules/Mavlink/UdpMavlinkModule.h b/src/shared/Modules/Mavlink/UdpMavlinkModule.h
new file mode 100644
index 00000000..e0a92fd3
--- /dev/null
+++ b/src/shared/Modules/Mavlink/UdpMavlinkModule.h
@@ -0,0 +1,43 @@
+/*
+ * This file is part of Skyward Hub.
+ *
+ * Skyward Hub 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.
+ *
+ * Skyward Hub 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
+ * Skyward Hub. If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include "BaseMavlinkModule.h"
+#include "Ports/UdpPort.h"
+
+class UdpMavlinkModule : public BaseMavlinkModule
+{
+ Q_OBJECT
+public:
+ explicit UdpMavlinkModule(QWidget *parent = nullptr);
+
+private:
+ void childSetupUi();
+ void childDisableControls() override;
+ void childEnableControls() override;
+
+ XmlObject childToXmlObject() override;
+ void childFromXmlObject(const XmlObject &obj) override;
+ bool open() override;
+
+ UdpPort udp;
+
+ QLineEdit *recvPort;
+ QLineEdit *sendPort;
+};
\ No newline at end of file
diff --git a/src/shared/Modules/ModuleInfo.h b/src/shared/Modules/ModuleInfo.h
index 75907750..57771cc8 100644
--- a/src/shared/Modules/ModuleInfo.h
+++ b/src/shared/Modules/ModuleInfo.h
@@ -32,7 +32,8 @@ enum ModuleId
GRAPH,
OUTCOMINGMESSAGEVIEWER,
INCOMINGMESSAGESVIEWER,
- MAVLINK,
+ SERIAL_MAVLINK,
+ UDP_MAVLINK,
FILESTREAM,
ORIENTATION_VISUALIZER,
STATEVIEWER,
diff --git a/src/shared/Modules/ModulesList.cpp b/src/shared/Modules/ModulesList.cpp
index 666fadd8..0d2cb472 100644
--- a/src/shared/Modules/ModulesList.cpp
+++ b/src/shared/Modules/ModulesList.cpp
@@ -26,7 +26,8 @@
#include <Modules/FileStream/FileStreamModule.h>
#include <Modules/Graph/Graph.h>
#include <Modules/IncomingMessagesViewer/IncomingMessagesViewerModule.h>
-#include <Modules/Mavlink/MavlinkModule.h>
+#include <Modules/Mavlink/SerialMavlinkModule.h>
+#include <Modules/Mavlink/UdpMavlinkModule.h>
#include <Modules/OrientationVisualizer/OrientationVisualizer.h>
#include <Modules/OutgoingMessagesViewer/OutgoingMessagesViewerModule.h>
#include <Modules/Splitter/Splitter.h>
@@ -94,10 +95,15 @@ void ModulesList::createModuleList()
inMsgViewer.setFactory([]() { return new IncomingMessagesViewerModule(); });
addModuleInfo(inMsgViewer);
- ModuleInfo mavlink(ModuleId::MAVLINK, "Mavlink",
- ModuleCategory::DATASOURCE);
- mavlink.setFactory([]() { return new MavlinkModule(); });
- addModuleInfo(mavlink);
+ ModuleInfo serialMavlink(ModuleId::SERIAL_MAVLINK, "SerialMavlink",
+ ModuleCategory::DATASOURCE);
+ serialMavlink.setFactory([]() { return new SerialMavlinkModule(); });
+ addModuleInfo(serialMavlink);
+
+ ModuleInfo udpMavlink(ModuleId::UDP_MAVLINK, "UdpMavlink",
+ ModuleCategory::DATASOURCE);
+ udpMavlink.setFactory([]() { return new UdpMavlinkModule(); });
+ addModuleInfo(udpMavlink);
ModuleInfo fileStream(ModuleId::FILESTREAM, "FileStream",
ModuleCategory::DATASOURCE);
--
GitLab