diff --git a/Modules/Mavlink/mavlinkcommandadapter.cpp b/Modules/Mavlink/mavlinkcommandadapter.cpp index 0e03a6fe83504fc5b4a9d52b9e69d2cd08838f4c..a92d8591f5b90b29a475fa6bb6ed3a5aa6eb6ab7 100644 --- a/Modules/Mavlink/mavlinkcommandadapter.cpp +++ b/Modules/Mavlink/mavlinkcommandadapter.cpp @@ -104,3 +104,49 @@ void MavlinkCommandAdapter::send(mavlink_message_t 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/Modules/Mavlink/mavlinkcommandadapter.h b/Modules/Mavlink/mavlinkcommandadapter.h index b79702bb3d0a75d763b2b718eb989bd279cd74c8..d271ae579ecd97c1cc937ceab584fa60e3229dc7 100644 --- a/Modules/Mavlink/mavlinkcommandadapter.h +++ b/Modules/Mavlink/mavlinkcommandadapter.h @@ -30,6 +30,8 @@ public: mavlink_message_t encode_RAW_EVENT_TC(uint8_t event_id, uint8_t topic_id); mavlink_message_t encode_UPLOAD_SETTING_TC(uint8_t setting_id, float setting_value); + bool produceMsgFromXml(const XmlObject &xml, mavlink_message_t *msg); + //void loadMsgDefinition(); signals: void publishRequested(const ModuleMessage &msg); diff --git a/Modules/Mavlink/mavlinkreader.cpp b/Modules/Mavlink/mavlinkreader.cpp index 38de0e23955af512f2f750c9f0a166fa65192a71..5d3355bbea384ff9b72f903194a8774a06b1b6b3 100644 --- a/Modules/Mavlink/mavlinkreader.cpp +++ b/Modules/Mavlink/mavlinkreader.cpp @@ -83,10 +83,10 @@ mavlink_message_t MavlinkReader::waitForMavlinkMsg() } -void MavlinkReader::parseMavlinkMsg(mavlink_message_t *msg) +QList<ModuleMessage> MavlinkReader::parseMavlinkMsg(mavlink_message_t *msg) { if(msg == nullptr) - return; + return QList<ModuleMessage>(); const mavlink_message_info_t *m = &message_info[msg->msgid]; const mavlink_field_info_t *f = m->fields; @@ -99,7 +99,8 @@ void MavlinkReader::parseMavlinkMsg(mavlink_message_t *msg) QString payload = extractFields(msg, &f[i]); fields[QString(f[i].name)] = payload; } - translateToGsMessage(msg, fields); + QString msgName = QString(m->name); + return translateToGsMessage(msg, msgName, fields); } QString MavlinkReader::extractFields(mavlink_message_t *msg, const mavlink_field_info_t *f) @@ -126,7 +127,7 @@ QString MavlinkReader::extractFields(mavlink_message_t *msg, const mavlink_field return payload; } -void MavlinkReader::translateToGsMessage(mavlink_message_t *msg, const QMap<QString,QString> &payloads) +QList<ModuleMessage> MavlinkReader::translateToGsMessage(mavlink_message_t *msg, const QString &msgName, const QMap<QString,QString> &payloads) { QDateTime timestamp; if(payloads.contains("timestamp")){ @@ -138,11 +139,15 @@ void MavlinkReader::translateToGsMessage(mavlink_message_t *msg, const QMap<QStr timestamp = timestamp.currentDateTime(); } + QList<ModuleMessage> messagesReceived; + for(QString fieldName : payloads.keys()){ - QString topic = QString::number(msg->msgid) + "/" + fieldName; + QString topic = msgName + "/" + fieldName; ModuleMessage msg(topic, payloads[fieldName],timestamp); emit msgReceived(msg); + messagesReceived.append(msg); } + return messagesReceived; } diff --git a/Modules/Mavlink/mavlinkreader.h b/Modules/Mavlink/mavlinkreader.h index d6b8088ad5b1b6c31282a607e5956c4b1723c8fc..2bfeaac605eca80cb80eb9a31caae3d30a19cc79 100644 --- a/Modules/Mavlink/mavlinkreader.h +++ b/Modules/Mavlink/mavlinkreader.h @@ -19,6 +19,7 @@ public: void close(); void startAsyncReading(QSerialPort *port); + QList<ModuleMessage> parseMavlinkMsg(mavlink_message_t *msg); signals: //void receivedMsg(const QString &topic,const QString &payload,const QDateTime ×tamp); @@ -27,14 +28,15 @@ signals: protected: //void loadMsgMapping(); mavlink_message_t waitForMavlinkMsg(); - void parseMavlinkMsg(mavlink_message_t *msg); + QString extractFields(mavlink_message_t *msg, const mavlink_field_info_t *f); QString extractOneField(mavlink_message_t *msg, const mavlink_field_info_t *f, int idx); + /* * This method emit the signal msgReceived */ - void translateToGsMessage(mavlink_message_t *msg, const QMap<QString, QString> &payloads); + QList<ModuleMessage> translateToGsMessage(mavlink_message_t *msg, const QString &msgName, const QMap<QString, QString> &payloads); void run(); private: diff --git a/Modules/Mavlink/mavlinkrocketmsgtestingmodule.cpp b/Modules/Mavlink/mavlinkrocketmsgtestingmodule.cpp index a6c2c097ac7cd7ae19a6aad2169378debb3a1d93..39138630d5eb9e3957687693141ee70dd9e970c1 100644 --- a/Modules/Mavlink/mavlinkrocketmsgtestingmodule.cpp +++ b/Modules/Mavlink/mavlinkrocketmsgtestingmodule.cpp @@ -6,6 +6,9 @@ #include "mavlinkversionheader.h" #include <QMap> #include <QDebug> +#include "mavlinkcommandadapter.h" +#include "mavlinkversionheader.h" +#include "mavlinkreader.h" MavlinkRocketMsgTestingModule::MavlinkRocketMsgTestingModule(QWidget *parent) : DefaultModule(parent), ui(new Ui::MavlinkRocketMsgTestingModule) { @@ -78,31 +81,6 @@ void MavlinkRocketMsgTestingModule::setSelectedMsg(const QString &msgName) } -void MavlinkRocketMsgTestingModule::onSendMsgClicked() -{ - QString currentTopic = getCurrentTopic().trimmed(); - if(currentMsgView != nullptr && currentTopic != ""){ - //this->getCore()->getModuleMessagesBroker()->publish(currentTopic, currentMsgView->text()); - } -} - -void MavlinkRocketMsgTestingModule::onLoadFileClicked() -{ - clearView(); - - QString filePath = SkywardHubStrings::defaultConfigurationFolder + "/" + ui->lineEdit_mavlinkInputFilePath->text().trimmed(); - if (mavlinkMsgDefinitionFile.loadFromFile(filePath)){ - QList<XmlObject*> xmlMessages = mavlinkMsgDefinitionFile.deepSearchObjects([this](const XmlObject *msg){ - if(msg->getObjectName() == xmlChildName){ - return true; - } - return false; - }); - - createViewFromXmlMessages(xmlMessages); - } -} - void MavlinkRocketMsgTestingModule::createViewFromXmlMessages(QList<XmlObject *> messagesList) { for (int i = 0; i < messagesList.count(); i++ ) { @@ -131,6 +109,17 @@ QGroupBox* MavlinkRocketMsgTestingModule::getOrCreateGBox(const QString &title) return gBox; } + +QString MavlinkRocketMsgTestingModule::getCurrentTopic() const +{ + return ui->lineEdit_outputTopic->text(); +} + +void MavlinkRocketMsgTestingModule::onRadioButtonClicked(QRadioButton *radiobutton) +{ + setSelectedMsg(radiobutton->text()); +} + void MavlinkRocketMsgTestingModule::clearView() { if(currentMsgView != nullptr){ @@ -146,16 +135,82 @@ void MavlinkRocketMsgTestingModule::clearView() viewGroupList.clear(); } -QString MavlinkRocketMsgTestingModule::getCurrentTopic() const +void MavlinkRocketMsgTestingModule::onLoadFileClicked() { - return ui->lineEdit_outputTopic->text(); + clearView(); + + QString filePath = SkywardHubStrings::defaultConfigurationFolder + "/" + ui->lineEdit_mavlinkInputFilePath->text().trimmed(); + if (mavlinkMsgDefinitionFile.loadFromFile(filePath)){ + QList<XmlObject*> xmlMessages = mavlinkMsgDefinitionFile.deepSearchObjects([this](const XmlObject *msg){ + if(msg->getObjectName() == xmlChildName){ + return true; + } + return false; + }); + + createViewFromXmlMessages(xmlMessages); + } } -void MavlinkRocketMsgTestingModule::onRadioButtonClicked(QRadioButton *radiobutton) +XmlObject MavlinkRocketMsgTestingModule::msgToXml() const { - setSelectedMsg(radiobutton->text()); + XmlObject xmlMsg; + if(currentMsgView != nullptr){ + xmlMsg.setObjectName(currentMsgView->getName()); + auto fields = currentMsgView->getFields(); + QMapIterator<QString, QLineEdit *> i(fields); + while (i.hasNext()) { + i.next(); + xmlMsg.addAttribute(i.key(), i.value()->text()); + } + } + return xmlMsg; +} + +void MavlinkRocketMsgTestingModule::onSendMsgClicked() +{ + if(currentMsgView != nullptr){ + XmlObject xmlMsg = msgToXml(); + testMavlinkEncodeAndDecode(xmlMsg); + + } } +bool MavlinkRocketMsgTestingModule::testMavlinkEncodeAndDecode(const XmlObject &xmlMsg) +{ + // Test mavlink msg encoding + mavlink_message_t mavMsg; + bool ok = false; + ok = MavlinkCommandAdapter().produceMsgFromXml(xmlMsg, &mavMsg); + + // Test of buffer writing +// if(MavlinkCommandAdapter().produceMsgFromXml(xmlMsg, &mavMsg)){ +// unsigned char buff[sizeof(mavlink_message_t)+1]; +// int msg_len = mavlink_msg_to_send_buffer(buff, &mavMsg); +// ok = msg_len > 0; +// } + + + // Test mavlink msg decoding + if(ok){ + MavlinkReader mavlinkReader; + QList<ModuleMessage> msgProduced = mavlinkReader.parseMavlinkMsg(&mavMsg); + + QString currentTopic = getCurrentTopic().trimmed(); + for (int i = 0; i < msgProduced.count() ; i++ ) { + getCore()->getModuleMessagesBroker()->publish(msgProduced[i]); + + if(currentTopic != ""){ + msgProduced[i].setTopic(currentTopic); + getCore()->getModuleMessagesBroker()->publish(msgProduced[i]); + } + } + } + return ok; +} + + + //mavlink_message_t MavlinkRocketMsgTestingModule::genericPack(string funcName, char* params[]) //{ // mavlink_message_t result_msg; @@ -176,6 +231,7 @@ void MavlinkRocketMsgTestingModule::onRadioButtonClicked(QRadioButton *radiobutt // return result_msg; //} + // _______________________________________________ MsgView ___________________________________________________ MsgView::MsgView(const XmlObject *obj, QString name) : QObject() @@ -243,6 +299,16 @@ QWidget *MsgView::getView() return view; } +QString MsgView::getName() const +{ + return name; +} + +QMap<QString, QLineEdit *> MsgView::getFields() const +{ + return fields; +} + diff --git a/Modules/Mavlink/mavlinkrocketmsgtestingmodule.h b/Modules/Mavlink/mavlinkrocketmsgtestingmodule.h index 2156c0e7b0d18aaca3a29b30beced71c2bd1f159..63ac08eee09629b31c3be63958834cceff4a0308 100644 --- a/Modules/Mavlink/mavlinkrocketmsgtestingmodule.h +++ b/Modules/Mavlink/mavlinkrocketmsgtestingmodule.h @@ -21,6 +21,10 @@ public: ~MsgView(); QWidget *getView(); + QString getName() const; + + QMap<QString, QLineEdit *> getFields() const; + private: QString description; QString name; @@ -55,11 +59,13 @@ protected: void setSelectedMsg(const QString &msgName); void onSendMsgClicked(); void onLoadFileClicked(); + bool testMavlinkEncodeAndDecode(const XmlObject &xmlMsg); // mavlink_message_t genericPack(string funcName, char *params[]); QString getCurrentTopic() const; void createViewFromXmlMessages(QList<XmlObject*> messagesList); QGroupBox* getOrCreateGBox(const QString &title); void clearView(); + XmlObject msgToXml() const; protected slots: void onRadioButtonClicked(QRadioButton *radiobutton); diff --git a/Modules/Table/tablemodule.cpp b/Modules/Table/tablemodule.cpp index bccc5e3d81fc9f91489a981e1b19d22b78d93c5d..de6ecd98d18b48fbaf2966a029f9ce0878662044 100644 --- a/Modules/Table/tablemodule.cpp +++ b/Modules/Table/tablemodule.cpp @@ -79,6 +79,16 @@ void TableModule::onSubscriptionRemoved(const QString &subscription) { subscriptions.removeOne(subscription); getCore()->getModuleMessagesBroker()->unsubscribe(subscription, this); + removeRow(subscription); +} + +void TableModule::removeRow(const QString &txt) +{ + int row = verticalHeaders.indexOf(txt); + if(row >= 0 ){ + verticalHeaders.removeAt(row); + ui->tableWidget->removeRow(row); + } } void TableModule::onMsgReceived(const ModuleMessage &msg) @@ -101,7 +111,7 @@ int TableModule::updateVerticalHeaders(const QString &txt) row = verticalHeaders.count() - 1; ui->tableWidget->insertRow(row); QTableWidgetItem *header = new QTableWidgetItem(txt); - ui->tableWidget->setVerticalHeaderItem(row,header); + ui->tableWidget->setVerticalHeaderItem(row, header); return row; } } @@ -128,6 +138,8 @@ int TableModule::updateHorizontalHeaders(const QString &txt) } } + + void TableModule::addCustomActionsToMenu() { QAction* subscribe = new QAction("Subscribe"); diff --git a/Modules/Table/tablemodule.h b/Modules/Table/tablemodule.h index 65385a3d230181076d0978359b63df61503c621c..252c8632aba07b572f4e81631bed6351d6d17921 100644 --- a/Modules/Table/tablemodule.h +++ b/Modules/Table/tablemodule.h @@ -36,6 +36,7 @@ protected: void connectUiSlots(); int updateVerticalHeaders(const QString &txt); int updateHorizontalHeaders(const QString &txt); + void removeRow(const QString &txt); void addCustomActionsToMenu() override; private: diff --git a/Modules/skywardhubstrings.h b/Modules/skywardhubstrings.h index 562e71399c5d0f1d614a7030cf86d1a9a8f027a9..9241e2b8e6b1c0db66b45845f09dfe02c63a5681 100644 --- a/Modules/skywardhubstrings.h +++ b/Modules/skywardhubstrings.h @@ -43,6 +43,8 @@ public: static const QString msgIdField; static const QString msgSequenceNumberField; + + static const QString mavlink_tester_publish_topic; }; diff --git a/SkywardHub.pro.user b/SkywardHub.pro.user index 868cd987652380b9f31eba18bda6afa58603aa2f..6ff847bea2578248a13044410f82927b661020ef 100644 --- a/SkywardHub.pro.user +++ b/SkywardHub.pro.user @@ -1,6 +1,6 @@ <?xml version="1.0" encoding="UTF-8"?> <!DOCTYPE QtCreatorProject> -<!-- Written by QtCreator 4.14.2, 2021-04-09T18:13:20. --> +<!-- Written by QtCreator 4.14.2, 2021-05-17T20:57:56. --> <qtcreator> <data> <variable>EnvironmentId</variable>