diff --git a/src/shared/Modules/RefuelingButton/RefuelingButton.cpp b/src/shared/Modules/RefuelingButton/RefuelingButton.cpp
index 4fe7993ae1b409aa2c756369cbe8b38258e2d535..46a4c1b3383b51cc1cf5b254172906690d643a17 100644
--- a/src/shared/Modules/RefuelingButton/RefuelingButton.cpp
+++ b/src/shared/Modules/RefuelingButton/RefuelingButton.cpp
@@ -19,8 +19,11 @@
#include "RefuelingButton.h"
#include <Core/Message/Message.h>
+#include <Modules/Mavlink/MavlinkCommandAdapter.h>
#include <Modules/RefuelingButton/MessagesListRefButton.h>
+#include <Modules/SkywardHubStrings.h>
+#include <QString>
#include <QVBoxLayout>
RefuelingButton::RefuelingButton(QWidget *parent) : DefaultModule(parent)
@@ -66,6 +69,105 @@ void RefuelingButton::fromXmlObject(const XmlObject &obj)
}
}
+Field RefuelingButton::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();
+ }
+ }
+}
+
+Field RefuelingButton::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();
+ }
+ }
+}
+
void RefuelingButton::setupUi()
{
QVBoxLayout *outerLayout = new QVBoxLayout;
@@ -112,18 +214,50 @@ void RefuelingButton::setupUi()
if (formElements.contains(key))
{
- uint64_t keyN = messagesListComboBox->currentIndex();
- auto message = formElements[key]->prepareMessage(key);
+ // uint64_t keyN = messagesListComboBox->currentIndex();
+ // auto message = formElements[key]->prepareMessage(key);
+
+ // if (messagesListComboBox->currentIndex() == 0 ||
+ // messagesListComboBox->currentIndex() == 1)
+ // message.setTopic(Topic{"Mav/MOTOR_TM"});
+
+ // if (messagesListComboBox->currentIndex() == 2 ||
+ // messagesListComboBox->currentIndex() == 3)
+ // message.setTopic(Topic{"Mav/GSE_TM"});
+
+ mavlink_motor_tm_t motor;
+ motor.bottom_tank_pressure = 0;
+ motor.combustion_chamber_pressure = 0;
+ motor.estimated_mass = 0;
+ motor.floating_level = 0;
+ motor.tank_temperature = 0;
+ motor.timestamp = 0;
+ motor.top_tank_pressure = 0;
+
+ mavlink_message_t msg;
+ // TODO magic numbers to substitute with
+ // MavlinkCommandAdapter ones
+ mavlink_msg_motor_tm_encode(171, 96, &msg, &motor);
+
+ // Once the message is created we can fill the fields
+ // according to the struct defined before
+ mavlink_message_info_t infos[256] = MAVLINK_MESSAGE_INFO;
+ mavlink_message_info_t info = infos[msg.msgid];
- if (messagesListComboBox->currentIndex() == 0 ||
- messagesListComboBox->currentIndex() == 1)
- message.setTopic(Topic{"Mav/MOTOR_TM"});
+ QMap<QString, Field> fields;
+ for (unsigned int i = 0; i < info.num_fields; i++)
+ {
+ fields[QString(info.fields[i].name)] =
+ decodeField(msg, info.fields[i]);
+ }
- if (messagesListComboBox->currentIndex() == 2 ||
- messagesListComboBox->currentIndex() == 3)
- message.setTopic(Topic{"Mav/GSE_TM"});
+ Message output;
+ output.setTopic(
+ Topic(SkywardHubStrings::mavlink_received_msg_topic +
+ "/" + info.name));
+ output.setFields(std::move(fields));
- getCore()->getMessageBroker()->publish(message);
+ getCore()->getMessageBroker()->publish(output);
}
});
}
diff --git a/src/shared/Modules/RefuelingButton/RefuelingButton.h b/src/shared/Modules/RefuelingButton/RefuelingButton.h
index 1b43cc962d83f261ceb5ed779aed50c1f234584e..0db12d96bd0847a01fa8a9c23ae63a95aebd279e 100644
--- a/src/shared/Modules/RefuelingButton/RefuelingButton.h
+++ b/src/shared/Modules/RefuelingButton/RefuelingButton.h
@@ -20,6 +20,7 @@
#include <Core/Message/Message.h>
#include <Modules/DefaultModule/DefaultModule.h>
+#include <Modules/Mavlink/MavlinkModule.h>
#include <Modules/RefuelingButton/MessageFormElementRefButton.h>
#include <tuple>
@@ -39,6 +40,10 @@ public:
private:
void setupUi();
+ Field decodeField(const mavlink_message_t& msg,
+ const mavlink_field_info_t& field);
+ Field decodeArrayElement(const mavlink_message_t& msg,
+ const mavlink_field_info_t& field, int idx);
QString currentMessage;
QComboBox* messagesListComboBox;