From 0f09d5d26c0ae2128cab3fd9652e8ab07adf13ab Mon Sep 17 00:00:00 2001
From: Riccardo Musso <riccardo.musso@skywarder.eu>
Date: Fri, 3 Jun 2022 21:13:43 +0200
Subject: [PATCH] Updated modules

---
 Modules/CommandPad/commandpad.cpp             | 236 ++++++++++++++++++
 Modules/CommandPad/commandpad.h               |  27 ++
 .../incomingmessagesviewermodule.cpp          |   1 +
 Modules/Mavlink/mavlinkcommandadapter.cpp     |   5 -
 Modules/Mavlink/mavlinkcommandadapter.h       |   2 -
 Modules/Mavlink/mavlinkmodule.cpp             |  19 +-
 Modules/Mavlink/mavlinkmodule.h               |   1 -
 Modules/moduleinfo.h                          |   5 -
 Modules/moduleslist.cpp                       |  10 +
 Modules/skywardhubstrings.cpp                 |   1 -
 Modules/skywardhubstrings.h                   |   1 -
 SkywardHub.pro                                |   2 +
 12 files changed, 277 insertions(+), 33 deletions(-)
 create mode 100644 Modules/CommandPad/commandpad.cpp
 create mode 100644 Modules/CommandPad/commandpad.h

diff --git a/Modules/CommandPad/commandpad.cpp b/Modules/CommandPad/commandpad.cpp
new file mode 100644
index 00000000..3ad65a42
--- /dev/null
+++ b/Modules/CommandPad/commandpad.cpp
@@ -0,0 +1,236 @@
+#include "commandpad.h"
+
+#include <QVBoxLayout>
+
+const QMap<QString, uint8_t> CommandPad::systemTMList {
+    { "MAV_SYS_ID", 1 },
+    { "MAV_FSM_ID", 2 },
+    { "MAV_PIN_OBS_ID", 3 },
+    { "MAV_LOGGER_ID", 4 },
+    { "MAV_MAVLINK_STATS", 5 },
+    { "MAV_TASK_STATS_ID", 6 },
+    { "MAV_DPL_ID", 7 },
+    { "MAV_ADA_ID", 8 },
+    { "MAV_NAS_ID", 9 },
+    { "MAV_CAN_ID", 10 },
+    { "MAV_FLIGHT_ID", 11 },
+    { "MAV_FLIGHT_STATS_ID", 12 },
+    { "MAV_SENSORS_STATE_ID", 13 }
+};
+
+const QMap<QString, uint8_t> CommandPad::sensorsTMList = {
+    { "MAV_GPS_ID", 1 },
+    { "MAV_BMX160_ID", 2 },
+    { "MAV_VN100_ID", 3 },
+    { "MAV_MPU9250_ID", 4 },
+    { "MAV_ADS_ID", 5 },
+    { "MAV_MS5803_ID", 6 },
+    { "MAV_CURRENT_SENSE_ID", 7 },
+    { "MAV_LIS3MDL_ID", 8 },
+    { "MAV_DPL_PRESS_ID", 9 },
+    { "MAV_STATIC_PRESS_ID", 10 },
+    { "MAV_PITOT_PRESS_ID", 11 },
+    { "MAV_BATTERY_VOLTAGE_ID", 12 },
+    { "MAV_STRAIN_GAUGE_ID", 13 },
+};
+
+const QMap<QString, uint8_t> CommandPad::mavCommandList = {
+    { "MAV_CMD_ARM", 1 },
+    { "MAV_CMD_DISARM", 2 },
+    { "MAV_CMD_FORCE_LAUNCH", 3 },
+    { "MAV_CMD_FORCE_LANDING", 4 },
+    { "MAV_CMD_FORCE_EXPULSION", 5 },
+    { "MAV_CMD_FORCE_MAIN", 6 },
+    { "MAV_CMD_START_LOGGING", 7 },
+    { "MAV_CMD_CLOSE_LOG", 8 },
+    { "MAV_CMD_FORCE_REBOOT", 9 },
+    { "MAV_CMD_TEST_MODE", 10 },
+    { "MAV_CMD_START_RECORDING", 11 },
+    { "MAV_CMD_STOP_RECORDING", 12 }
+};
+
+const QMap<QString, uint8_t> CommandPad::servosList = {
+    { "AIRBRAKES_SERVO", 1 },
+    { "EXPULSION_SERVO", 2 },
+    { "PARAFOIL_SERVO1", 3 },
+    { "PARAFOIL_SERVO2", 4 },
+};
+
+
+CommandPad::CommandPad(QWidget* parent) : DefaultModule(parent) {
+    setupUi();
+    defaultContextMenuSetup();
+}
+
+CommandPad::~CommandPad() {
+
+}
+
+QWidget* CommandPad::toWidget() {
+    return this;
+}
+
+XmlObject CommandPad::toXmlObject() {
+    return XmlObject(getName(ModuleId::COMMANDPAD));
+}
+
+void CommandPad::fromXmlObject(const XmlObject& xmlObject) {
+    Q_UNUSED(xmlObject);
+}
+
+#define _CMD(id, strName) \
+    QWidget* id = new QWidget; \
+    QFormLayout* id##_form = new QFormLayout; \
+    id->setLayout(id##_form); \
+    stacked->addWidget(id); \
+    commandComboBox->addItem(strName); \
+
+#define _CMD_FLOAT_PROP(id, idProperty) \
+    QLineEdit* id##_##idProperty = new QLineEdit; \
+    id##_##idProperty->setValidator(new QDoubleValidator(-10000.0, 10000.0, 3, this)); \
+    id##_##idProperty->setSizePolicy(QSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed)); \
+    id##_form->addRow(tr(#idProperty ": "), id##_##idProperty);
+
+#define _CMD_UINT8_PROP(id, idProperty) \
+    QLineEdit* id##_##idProperty = new QLineEdit; \
+    id##_##idProperty->setValidator(new QIntValidator(0, 255, this)); \
+    id##_##idProperty->setSizePolicy(QSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed)); \
+    id##_form->addRow(tr(#idProperty ": "), id##_##idProperty);
+
+#define _CMD_COMBO_PROP(id, idProperty, map) \
+    QComboBox* id##_##idProperty = new QComboBox; \
+    id##_##idProperty->setSizePolicy(QSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed)); \
+    id##_##idProperty->insertItems(0, map.keys()); \
+    id##_form->addRow(tr(#idProperty ": "), id##_##idProperty);
+
+#define _CMD_END
+
+#define _SEND(id, strName) \
+    if(commandComboBox->currentText() == strName){ \
+        QTime time = QTime::currentTime(); \
+        ModuleMessage message(SkywardHubStrings::commandsTopic + "/" + strName); \
+        message.setField("timestamp", (uint64_t) time.msecsSinceStartOfDay());
+
+#define _SEND_FLOAT_PROP(id, idProperty) \
+        message.setField(#idProperty, id##_##idProperty->text().toDouble());
+
+#define _SEND_UINT8_PROP(id, idProperty) \
+        message.setField(#idProperty, (uint64_t) id##_##idProperty->text().toInt());
+
+#define _SEND_COMBO_PROP(id, idProperty, map) \
+        message.setField(#idProperty, (uint64_t) map[id##_##idProperty->currentText()]);
+
+#define _SEND_END \
+        getCore()->getModuleMessagesBroker()->publish(message); \
+    }
+
+void CommandPad::setupUi() {
+    QStackedWidget* stacked = new QStackedWidget;
+
+    QComboBox* commandComboBox = new QComboBox;
+    commandComboBox->setSizePolicy(QSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed));
+    connect(commandComboBox, QOverload<int>::of(&QComboBox::currentIndexChanged), this,
+    [ = ](int idx) {
+        stacked->setCurrentIndex(idx);
+    });
+
+    QFormLayout* form = new QFormLayout;
+    form->addRow(tr("Select command: "), commandComboBox);
+
+    _CMD(ping, "PING_TC")
+    _CMD_END
+    _CMD(command, "COMMAND_TC")
+    _CMD_COMBO_PROP(command, command_id, mavCommandList)
+    _CMD_END
+    _CMD(systemTelemetry, "SYSTEM_TELEMETRY_REQUEST_TC")
+    _CMD_COMBO_PROP(systemTelemetry, tm_id, systemTMList)
+    _CMD_END
+    _CMD(sensorTelemetry, "SENSOR_TELEMETRY_REQUEST_TC")
+    _CMD_COMBO_PROP(sensorTelemetry, sensor_id, sensorsTMList)
+    _CMD_END
+    _CMD(setServoAngle, "SET_SERVO_ANGLE_TC")
+    _CMD_COMBO_PROP(setServoAngle, servo_id, servosList)
+    _CMD_FLOAT_PROP(setServoAngle, angle)
+    _CMD_END
+    _CMD(wiggleServo, "WIGGLE_SERVO_TC")
+    _CMD_COMBO_PROP(wiggleServo, servo_id, servosList)
+    _CMD_END
+    _CMD(resetServo, "RESET_SERVO_TC")
+    _CMD_COMBO_PROP(resetServo, servo_id, servosList)
+    _CMD_END
+    _CMD(setReferenceAltitude, "SET_REFERENCE_ALTITUDE_TC")
+    _CMD_FLOAT_PROP(setReferenceAltitude, ref_altitude)
+    _CMD_END
+    _CMD(setReferenceTemperature, "SET_REFERENCE_TEMPERATURE_TC")
+    _CMD_FLOAT_PROP(setReferenceTemperature, ref_temp)
+    _CMD_END
+    _CMD(setDeploymentAltitude, "SET_DEPLOYMENT_ALTITUDE_TC")
+    _CMD_FLOAT_PROP(setDeploymentAltitude, dpl_altitude)
+    _CMD_END
+    _CMD(setInitialOrientation, "SET_INITIAL_ORIENTATION_TC")
+    _CMD_FLOAT_PROP(setInitialOrientation, yaw)
+    _CMD_FLOAT_PROP(setInitialOrientation, pitch)
+    _CMD_FLOAT_PROP(setInitialOrientation, roll)
+    _CMD_END
+    _CMD(setInitialCoordinates, "SET_INITIAL_COORDINATES_TC")
+    _CMD_FLOAT_PROP(setInitialCoordinates, latitude)
+    _CMD_FLOAT_PROP(setInitialCoordinates, longitude)
+    _CMD_END
+    _CMD(rawEvent, "RAW_EVENT_TC")
+    _CMD_UINT8_PROP(rawEvent, topic_id)
+    _CMD_UINT8_PROP(rawEvent, event_id)
+    _CMD_END
+
+    QPushButton* send = new QPushButton(tr("Send"));
+    connect(send, &QPushButton::clicked, [ = ]() {
+        _SEND(ping, "PING_TC")
+        _SEND_END
+        _SEND(command, "COMMAND_TC")
+        _SEND_COMBO_PROP(command, command_id, mavCommandList)
+        _SEND_END
+        _SEND(systemTelemetry, "SYSTEM_TELEMETRY_REQUEST_TC")
+        _SEND_COMBO_PROP(systemTelemetry, tm_id, systemTMList)
+        _SEND_END
+        _SEND(sensorTelemetry, "SENSOR_TELEMETRY_REQUEST_TC")
+        _SEND_COMBO_PROP(sensorTelemetry, sensor_id, sensorsTMList)
+        _SEND_END
+        _SEND(setServoAngle, "SET_SERVO_ANGLE_TC")
+        _SEND_COMBO_PROP(setServoAngle, servo_id, servosList)
+        _SEND_FLOAT_PROP(setServoAngle, angle)
+        _SEND_END
+        _SEND(wiggleServo, "WIGGLE_SERVO_TC")
+        _SEND_COMBO_PROP(wiggleServo, servo_id, servosList)
+        _SEND_END
+        _SEND(resetServo, "RESET_SERVO_TC")
+        _SEND_COMBO_PROP(resetServo, servo_id, servosList)
+        _SEND_END
+        _SEND(setReferenceAltitude, "SET_REFERENCE_ALTITUDE_TC")
+        _SEND_FLOAT_PROP(setReferenceAltitude, ref_altitude)
+        _SEND_END
+        _SEND(setReferenceTemperature, "SET_REFERENCE_TEMPERATURE_TC")
+        _SEND_FLOAT_PROP(setReferenceTemperature, ref_temp)
+        _SEND_END
+        _SEND(setDeploymentAltitude, "SET_DEPLOYMENT_ALTITUDE_TC")
+        _SEND_FLOAT_PROP(setDeploymentAltitude, dpl_altitude)
+        _SEND_END
+        _SEND(setInitialOrientation, "SET_INITIAL_ORIENTATION_TC")
+        _SEND_FLOAT_PROP(setInitialOrientation, yaw)
+        _SEND_FLOAT_PROP(setInitialOrientation, pitch)
+        _SEND_FLOAT_PROP(setInitialOrientation, roll)
+        _SEND_END
+        _SEND(setInitialCoordinates, "SET_INITIAL_COORDINATES_TC")
+        _SEND_FLOAT_PROP(setInitialCoordinates, latitude)
+        _SEND_FLOAT_PROP(setInitialCoordinates, longitude)
+        _SEND_END
+        _SEND(rawEvent, "RAW_EVENT_TC")
+        _SEND_UINT8_PROP(rawEvent, topic_id)
+        _SEND_UINT8_PROP(rawEvent, event_id)
+        _SEND_END
+    });
+
+    QVBoxLayout* layout = new QVBoxLayout;
+    layout->addLayout(form);
+    layout->addWidget(stacked);
+    layout->addWidget(send);
+    setLayout(layout);
+}
diff --git a/Modules/CommandPad/commandpad.h b/Modules/CommandPad/commandpad.h
new file mode 100644
index 00000000..a179cfc0
--- /dev/null
+++ b/Modules/CommandPad/commandpad.h
@@ -0,0 +1,27 @@
+#ifndef COMMANDPAD_H
+#define COMMANDPAD_H
+
+#include "Modules/DefaultModule/defaultmodule.h"
+
+class CommandPad : public DefaultModule {
+    Q_OBJECT
+
+  public:
+    static const QMap<QString, uint8_t> systemTMList;
+    static const QMap<QString, uint8_t> sensorsTMList;
+    static const QMap<QString, uint8_t> mavCommandList;
+    static const QMap<QString, uint8_t> servosList;
+
+    explicit CommandPad(QWidget* parent = nullptr);
+    ~CommandPad();
+
+    QWidget* toWidget() override;
+
+    XmlObject toXmlObject() override;
+    void fromXmlObject(const XmlObject& xmlObject) override;
+
+  private:
+    void setupUi();
+};
+
+#endif // COMMANDPAD_H
diff --git a/Modules/IncomingMessagesViewer/incomingmessagesviewermodule.cpp b/Modules/IncomingMessagesViewer/incomingmessagesviewermodule.cpp
index 3b546954..2a3b5ec3 100644
--- a/Modules/IncomingMessagesViewer/incomingmessagesviewermodule.cpp
+++ b/Modules/IncomingMessagesViewer/incomingmessagesviewermodule.cpp
@@ -85,6 +85,7 @@ void IncomingMessagesViewerModule::addCustomActionsToMenu() {
     check->setChecked(clearEveryMessage);
     connect(check, &QAction::toggled, this, &IncomingMessagesViewerModule::setClearEveryMessage);
 
+    addActionToMenu(clear);
     addActionToMenu(manage);
     addActionToMenu(check);
 }
diff --git a/Modules/Mavlink/mavlinkcommandadapter.cpp b/Modules/Mavlink/mavlinkcommandadapter.cpp
index d7dfb046..ce26583e 100644
--- a/Modules/Mavlink/mavlinkcommandadapter.cpp
+++ b/Modules/Mavlink/mavlinkcommandadapter.cpp
@@ -16,11 +16,6 @@ bool MavlinkCommandAdapter::encodeCommand(const ModuleMessage& msg, mavlink_mess
     return true;
 }
 
-bool MavlinkCommandAdapter::encodeTelemetryRequest(const ModuleMessage& msg, mavlink_message_t& output) {
-    return true;
-}
-
-
 void MavlinkCommandAdapter::send(mavlink_message_t msg) {
     ModuleMessage response(SkywardHubStrings::logCommandsTopic);
     response.setField(SkywardHubStrings::msgIdField, MessageField((uint64_t) msg.msgid));
diff --git a/Modules/Mavlink/mavlinkcommandadapter.h b/Modules/Mavlink/mavlinkcommandadapter.h
index c3cfdc2d..5e16ee10 100644
--- a/Modules/Mavlink/mavlinkcommandadapter.h
+++ b/Modules/Mavlink/mavlinkcommandadapter.h
@@ -22,9 +22,7 @@ class MavlinkCommandAdapter : public QObject {
 
     void send(mavlink_message_t msg);
     void setSerialPort(QSerialPort* value);
-
     bool encodeCommand(const ModuleMessage& msg, mavlink_message_t& output);
-    bool encodeTelemetryRequest(const ModuleMessage& msg, mavlink_message_t& output);
 
     bool produceMsgFromXml(const XmlObject& xml, mavlink_message_t* msg);
     static const int MAV_CMP = 96;
diff --git a/Modules/Mavlink/mavlinkmodule.cpp b/Modules/Mavlink/mavlinkmodule.cpp
index 0e5c4076..985581b6 100644
--- a/Modules/Mavlink/mavlinkmodule.cpp
+++ b/Modules/Mavlink/mavlinkmodule.cpp
@@ -50,13 +50,9 @@ void MavlinkModule::fromXmlObject(const XmlObject& xmlObject) {
 }
 
 void MavlinkModule::subscribe() {
-    getCore()->getModuleMessagesBroker()->subscribe(SkywardHubStrings::commandsTopic, this, [this](const ModuleMessage & msg) {
+    getCore()->getModuleMessagesBroker()->subscribe({SkywardHubStrings::commandsTopic + "/*"}, this, [this](const ModuleMessage & msg) {
         onCommandReceived(msg);
     });
-
-    getCore()->getModuleMessagesBroker()->subscribe(SkywardHubStrings::telemetryRequestTopic, this, [this](const ModuleMessage & msg) {
-        onTelemetryRequestReceived(msg);
-    });
 }
 
 void MavlinkModule::publish(const ModuleMessage& msg) {
@@ -211,16 +207,3 @@ void MavlinkModule::onCommandReceived(const ModuleMessage& msg) {
         mavlinkCommandAdapter.send(encoded_mvl_msg);
     }
 }
-
-void MavlinkModule::onTelemetryRequestReceived(const ModuleMessage& msg) {
-    mavlink_message_t encoded_mvl_msg;
-    if(!mavlinkCommandAdapter.encodeTelemetryRequest(msg, encoded_mvl_msg)) {
-        qDebug() << "UNRECOGNIZED TELEMETRY REQUEST FROM COMMAND ADAPTER!!!";
-    } else if(serialPort.isOpen()) {
-        mavlinkCommandAdapter.send(encoded_mvl_msg);
-    }
-
-    if(serialPort.isOpen()) {
-        mavlinkCommandAdapter.send(encoded_mvl_msg);
-    }
-}
diff --git a/Modules/Mavlink/mavlinkmodule.h b/Modules/Mavlink/mavlinkmodule.h
index 5e6062ef..2dbe6135 100644
--- a/Modules/Mavlink/mavlinkmodule.h
+++ b/Modules/Mavlink/mavlinkmodule.h
@@ -29,7 +29,6 @@ class MavlinkModule : public DefaultModule {
     void fromXmlObject(const XmlObject& xmlObject) override;
 
     void onCommandReceived(const ModuleMessage& msg);
-    void onTelemetryRequestReceived(const ModuleMessage& msg);
 
   public slots:
     void onStartClicked();
diff --git a/Modules/moduleinfo.h b/Modules/moduleinfo.h
index 0e28d220..d8b6795c 100644
--- a/Modules/moduleinfo.h
+++ b/Modules/moduleinfo.h
@@ -10,16 +10,11 @@ enum ModuleId {
     SPLITTER,
     SKYWARDHUB,
     COMMANDPAD,
-    TELEMETRYPAD,
     BROKERTEST,
     GRAPH,
-    TABLE,
     OUTCOMINGMESSAGEVIEWER,
     INCOMINGMESSAGESVIEWER,
     MAVLINK,
-    FSMVIEWER,
-    TREEVIEWER,
-    SCROLLAREA,
     FILESTREAM,
     STATEVIEWER,
     VALUESCONVERTERVIEWER,
diff --git a/Modules/moduleslist.cpp b/Modules/moduleslist.cpp
index 1cf04de4..2431ae63 100644
--- a/Modules/moduleslist.cpp
+++ b/Modules/moduleslist.cpp
@@ -4,6 +4,7 @@
 
 #include "Modules/Empty/emptymodule.h"
 #include "Components/ModulesPicker/modulespicker.h"
+#include "Modules/CommandPad/commandpad.h"
 #include "Modules/Splitter/splittermodule.h"
 #include "Modules/SkywardHub/skywardhubmodule.h"
 #include "Modules/Test/testmodule.h"
@@ -41,6 +42,15 @@ void ModulesList::createModuleList() {
     addModuleInfo(empty);
 #endif
 
+#ifdef COMMANDPAD_H
+    ModuleInfo commandPad(ModuleId::COMMANDPAD, "CommandPad", ModuleCategory::UTILITY);
+    commandPad.setFactory([]() {
+        return new CommandPad();
+    });
+    commandPad.addModuleSourceFiles("Modules/CommandPad/");
+    addModuleInfo(commandPad);
+#endif
+
 #ifdef SPLITTERMODULE_H
     ModuleInfo splitter(ModuleId::SPLITTER, "Splitter");
     splitter.setFactory([]() {
diff --git a/Modules/skywardhubstrings.cpp b/Modules/skywardhubstrings.cpp
index 4e35e63e..baff6b0b 100644
--- a/Modules/skywardhubstrings.cpp
+++ b/Modules/skywardhubstrings.cpp
@@ -28,7 +28,6 @@ const QString SkywardHubStrings::imageViewerInfo = "Dobule click on a label to o
 
 // Topics
 const QString SkywardHubStrings::commandsTopic = "TelemetryCommand";
-const QString SkywardHubStrings::telemetryRequestTopic = "TelemetryRequest";
 const QString SkywardHubStrings::treeViewerReceivingBaseTopic = "TreeViewer";
 
 const QString SkywardHubStrings::logCommandsTopic = "LogCommands";
diff --git a/Modules/skywardhubstrings.h b/Modules/skywardhubstrings.h
index 1eb87d97..ce45b06a 100644
--- a/Modules/skywardhubstrings.h
+++ b/Modules/skywardhubstrings.h
@@ -33,7 +33,6 @@ class SkywardHubStrings {
 
     // Topics
     static const QString commandsTopic;
-    static const QString telemetryRequestTopic;
     static const QString treeViewerReceivingBaseTopic;
 
     static const QString logCommandsTopic;
diff --git a/SkywardHub.pro b/SkywardHub.pro
index 0fa8eece..5da85fce 100644
--- a/SkywardHub.pro
+++ b/SkywardHub.pro
@@ -29,6 +29,7 @@ SOURCES += \
     Core/modulesmanager.cpp \
     Core/skywardhubcore.cpp \
     Core/xmlobject.cpp \
+    Modules/CommandPad/commandpad.cpp \
     Modules/DefaultModule/defaultmodule.cpp \
     Modules/Empty/emptymodule.cpp \
     Modules/FileStream/filestreammodule.cpp \
@@ -81,6 +82,7 @@ HEADERS += \
     Core/modulesmanager.h \
     Core/skywardhubcore.h \
     Core/xmlobject.h \
+    Modules/CommandPad/commandpad.h \
     Modules/DefaultModule/defaultmodule.h \
     Modules/Empty/emptymodule.h \
     Modules/FileStream/filestreammodule.h \
-- 
GitLab