Skip to content
Snippets Groups Projects
Commit d619ca1a authored by Riccardo Musso's avatar Riccardo Musso
Browse files

Various updates

parent 9d431358
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Modules/CommandPad/commandpadmodule.h
+ 8
− 8
View file @ d619ca1a
#ifndef COMMANDPADMODULE_H
#define COMMANDPADMODULE_H
#include "Core/modulemessage.h"
#include "Core/Message/modulemessage.h"
#include "Modules/DefaultModule/defaultmodule.h"
namespace Ui {
......
Modules/Mavlink/mavlinkreader.cpp
+ 29
− 69
View file @ d619ca1a
......@@ -41,7 +41,7 @@ void MavlinkReader::onReadyRead() {
auto bytes = serialPort->readAll();
for(auto it = bytes.begin(); it != bytes.end(); ++it) {
if(mavlink_parse_char(MAVLINK_COMM_0, *it, &decodedMessage, &mavlinkStatus)) {
parseMavlinkMsg(decodedMessage);
emit messageReceived(generateModuleMessage(decodedMessage));
}
}
......@@ -54,41 +54,18 @@ void MavlinkReader::setSerialPort(QSerialPort* port) {
serialPort = port;
}
QList<ModuleMessage> MavlinkReader::parseMavlinkMsg(const mavlink_message_t& msg) {
ModuleMessage MavlinkReader::generateModuleMessage(const mavlink_message_t& msg) {
const mavlink_message_info_t& info = getMessageFormat(msg.msgid);
QMap<QString, QString> fields;
QMap<QString, MessageField> fields;
for (unsigned i = 0; i < info.num_fields; i++) {
QString payload = decodeField(msg, info.fields[i]);
fields[QString(info.fields[i].name)] = payload;
fields[QString(info.fields[i].name)] = decodeField(msg, info.fields[i]);
}
QString msgName = QString(info.name);
return translateToGsMessage(msgName, fields);
}
QList<ModuleMessage> MavlinkReader::translateToGsMessage(const QString& msgName, const QMap<QString, QString>& payloads) {
QTime timestamp(0, 0, 0, 0);
bool timeOK = false;
double t;
if(payloads.contains("timestamp")) {
t = payloads["timestamp"].toDouble(&timeOK);
}
if(timeOK) {
timestamp = timestamp.addMSecs(t);
}
QList<ModuleMessage> messagesReceived;
for(auto it = payloads.keyBegin(); it != payloads.keyEnd(); ++it) {
QString topic = SkywardHubStrings::mavlink_received_msg_topic + "/" + msgName + "/" + *it;
ModuleMessage msg(topic, payloads[*it], timestamp);
emit msgReceived(msg);
messagesReceived.append(msg);
}
return messagesReceived;
ModuleMessage output;
output.setTopic(SkywardHubStrings::mavlink_received_msg_topic + "/" + info.name);
output.setFields(std::move(fields));
return output;
}
void MavlinkReader::closeLog() {
......@@ -129,78 +106,61 @@ void MavlinkReader::setLogFilePath() {
logFilePath = proposedFilePath + fileNumber + extension;
}
QString MavlinkReader::decodeField(const mavlink_message_t& msg, const mavlink_field_info_t& field) {
QString payload;
MessageField MavlinkReader::decodeField(const mavlink_message_t& msg, const mavlink_field_info_t& field) {
if (field.array_length == 0) {
payload.append(decodeSubField(msg, field, 0));
return decodeArrayElement(msg, field, 0);
} else {
if (field.type == MAVLINK_TYPE_CHAR) {
payload.append(QString((const char*)_MAV_PAYLOAD(&msg)));
return MessageField(QString((const char*)_MAV_PAYLOAD(&msg)));
} else {
QList<MessageField> array;
array.reserve(field.array_length);
for (unsigned i = 0; i < field.array_length; i++) {
payload.append(decodeSubField(msg, field, i));
array.append(decodeArrayElement(msg, field, i));
}
return MessageField(std::move(array));
}
}
return payload;
}
QString MavlinkReader::decodeSubField(const mavlink_message_t& msg, const mavlink_field_info_t& field, int idx) {
QString payload = "";
MessageField MavlinkReader::decodeArrayElement(const mavlink_message_t& msg, const mavlink_field_info_t& field, int idx) {
switch (field.type) {
case MAVLINK_TYPE_CHAR: {
char val_char = _MAV_RETURN_char(&msg, field.wire_offset + idx * 1);
return QString(val_char);
return MessageField(_MAV_RETURN_char(&msg, field.wire_offset + idx * 1));
}
case MAVLINK_TYPE_UINT8_T: {
uint8_t val_uint8 = _MAV_RETURN_uint8_t(&msg, field.wire_offset + idx * 1);
return QString::number(val_uint8);
return MessageField(static_cast<uint64_t>(_MAV_RETURN_uint8_t(&msg, field.wire_offset + idx * 1)));
}
case MAVLINK_TYPE_INT8_T: {
int8_t val_int8 = _MAV_RETURN_int8_t(&msg, field.wire_offset + idx * 1);
return QString::number(val_int8);
return MessageField(static_cast<int64_t>(_MAV_RETURN_int8_t(&msg, field.wire_offset + idx * 1)));
}
case MAVLINK_TYPE_UINT16_T: {
uint16_t val_uint16 = _MAV_RETURN_uint16_t(&msg, field.wire_offset + idx * 2);
return QString::number(val_uint16);
return MessageField(static_cast<uint64_t>(_MAV_RETURN_uint16_t(&msg, field.wire_offset + idx * 2)));
}
case MAVLINK_TYPE_INT16_T: {
int16_t val_int16 = _MAV_RETURN_int16_t(&msg, field.wire_offset + idx * 2);
return QString::number(val_int16);
return MessageField(static_cast<int64_t>(_MAV_RETURN_int16_t(&msg, field.wire_offset + idx * 2)));
}
case MAVLINK_TYPE_UINT32_T: {
unsigned long val_uint32 = (unsigned long)_MAV_RETURN_uint32_t(&msg, field.wire_offset + idx * 4);
return QString::number(val_uint32);
return MessageField(static_cast<uint64_t>(_MAV_RETURN_uint32_t(&msg, field.wire_offset + idx * 4)));
}
case MAVLINK_TYPE_INT32_T: {
long val_int32 = (long)_MAV_RETURN_int32_t(&msg, field.wire_offset + idx * 4);
return QString::number(val_int32);
return MessageField(static_cast<int64_t>(_MAV_RETURN_int32_t(&msg, field.wire_offset + idx * 4)));
}
case MAVLINK_TYPE_UINT64_T: {
unsigned long long val_uint64 = (unsigned long long)_MAV_RETURN_uint64_t(&msg, field.wire_offset + idx * 8);
return QString::number(val_uint64);
return MessageField(static_cast<uint64_t>(_MAV_RETURN_uint64_t(&msg, field.wire_offset + idx * 8)));
}
case MAVLINK_TYPE_INT64_T: {
long long val_int64 = (long long)_MAV_RETURN_int64_t(&msg, field.wire_offset + idx * 8);
return QString::number(val_int64);
return MessageField(static_cast<int64_t>(_MAV_RETURN_int64_t(&msg, field.wire_offset + idx * 8)));
}
case MAVLINK_TYPE_FLOAT: {
double val_double = (double)_MAV_RETURN_float(&msg, field.wire_offset + idx * 4);
QString out;
QTextStream stream(&out);
stream.setRealNumberNotation(QTextStream::FixedNotation);
stream << val_double;
return out;
return MessageField(static_cast<double>(_MAV_RETURN_float(&msg, field.wire_offset + idx * 4)));
}
case MAVLINK_TYPE_DOUBLE: {
double val_double2 = _MAV_RETURN_double(&msg, field.wire_offset + idx * 8);
return QString::number(val_double2);
return MessageField(static_cast<double>(_MAV_RETURN_double(&msg, field.wire_offset + idx * 8)));
}
default: {
return "<unsupported>";
// Unsupported: return EMPTY
return MessageField();
}
}
}
Modules/Mavlink/mavlinkreader.h
+ 6
− 8
View file @ d619ca1a
......@@ -7,7 +7,7 @@
#include "Core/xmlobject.h"
#include "mavlinkversionheader.h"
#include "Core/modulemessage.h"
#include "Core/Message/modulemessage.h"
#include <QFile>
......@@ -20,7 +20,7 @@ class MavlinkReader : public QObject {
void startReading();
void stopReading();
QList<ModuleMessage> parseMavlinkMsg(const mavlink_message_t& msg);
ModuleMessage generateModuleMessage(const mavlink_message_t& msg);
void setSerialPort(QSerialPort* port);
void closeLog();
......@@ -29,22 +29,20 @@ class MavlinkReader : public QObject {
static const mavlink_message_info_t& getMessageFormat(uint8_t messageId);
signals:
void msgReceived(const ModuleMessage &msg);
void messageReceived(const ModuleMessage& msg);
private slots:
void onReadyRead();
protected:
/* convert a field of any type to a string */
QString decodeField(const mavlink_message_t& msg, const mavlink_field_info_t& field);
MessageField decodeField(const mavlink_message_t& msg, const mavlink_field_info_t& field);
/* if field is an array, decode every element in the array */
QString decodeSubField(const mavlink_message_t& msg, const mavlink_field_info_t& field, int idx);
MessageField decodeArrayElement(const mavlink_message_t& msg, const mavlink_field_info_t& field, int idx);
void setLogFilePath();
QList<ModuleMessage> translateToGsMessage(const QString &msgName, const QMap<QString, QString> &payloads);
private:
mavlink_message_t decodedMessage;
mavlink_status_t mavlinkStatus;
......
SkywardHub.pro
+ 2
− 0
View file @ d619ca1a
......@@ -20,6 +20,7 @@ SOURCES += \
Core/Message/messagefield.cpp \
Core/Message/modulemessage.cpp \
Core/Message/topic.cpp \
Core/Message/topicfilter.cpp \
Core/module.cpp \
Core/moduleeventshandler.cpp \
Core/modulemessagesbroker.cpp \
......@@ -78,6 +79,7 @@ HEADERS += \
Core/Message/messagefield.h \
Core/Message/modulemessage.h \
Core/Message/topic.h \
Core/Message/topicfilter.h \
Core/Messages/messagefield.h \
Core/Messages/modulemessage.h \
Core/Messages/topic.h \
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment