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