diff --git a/.clang-format b/.clang-format index 48706ce9f1752d16ad4942f7bbc7d1bf37d0fb61..43c7648a3b8b1497c07e324f7d289692fa3ce88c 100644 --- a/.clang-format +++ b/.clang-format @@ -1,13 +1,4 @@ { BasedOnStyle: Google, - AccessModifierOffset: -4, - AlignConsecutiveAssignments: true, - AllowShortIfStatementsOnASingleLine: false, - AllowShortLoopsOnASingleLine: false, - BreakBeforeBraces: Allman, - ColumnLimit: 160, - ConstructorInitializerAllOnOneLineOrOnePerLine: false, - IndentCaseLabels: true, IndentWidth: 4, - KeepEmptyLinesAtTheStartOfBlocks: true, } diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 432d41dd0eb6d0b5e8183fc817b44ddd0dec3023..c53785838aad821c4a2754227f28eadad970a4ee 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -4,7 +4,8 @@ "name": "Linux", "includePath": [ "${workspaceFolder}/**", - "/home/alberton/Qt/5.15.2/gcc_64/include/**" + "~/Qt/5.15.2/gcc_64/include/**", + "/Users/alberton/Qt/5.15.2/clang_64/include/**" ], "defines": [], "compilerPath": "/usr/bin/gcc", diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000000000000000000000000000000000000..0dc8485d70a8b9adafe9b3c138a299d38e639e26 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,7 @@ +{ + "files.associations": { + "ratio": "cpp", + "sstream": "cpp" + }, + "editor.defaultFormatter": "chiehyu.vscode-astyle" +} \ No newline at end of file diff --git a/Modules/CommandPad/commandpad.cpp b/Modules/CommandPad/commandpad.cpp index 9e1402ae0ab05ec61b968f3939298c95d18a71c8..10b37882440bc37dfb22d35f182af09809c655ef 100644 --- a/Modules/CommandPad/commandpad.cpp +++ b/Modules/CommandPad/commandpad.cpp @@ -2,146 +2,135 @@ #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_STATS_ID", 12 }, - { "MAV_SENSORS_STATE_ID", 13 } -}; +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_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_BME280_ID", 7 }, - { "MAV_CURRENT_SENSE_ID", 8 }, - { "MAV_LIS3MDL_ID", 9 }, - { "MAV_DPL_PRESS_ID", 10 }, - { "MAV_STATIC_PRESS_ID", 11 }, - { "MAV_PITOT_PRESS_ID", 12 }, - { "MAV_BATTERY_VOLTAGE_ID", 13 }, - { "MAV_STRAIN_GAUGE_ID", 14 }, + {"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_BME280_ID", 7}, + {"MAV_CURRENT_SENSE_ID", 8}, + {"MAV_LIS3MDL_ID", 9}, + {"MAV_DPL_PRESS_ID", 10}, + {"MAV_STATIC_PRESS_ID", 11}, + {"MAV_PITOT_PRESS_ID", 12}, + {"MAV_BATTERY_VOLTAGE_ID", 13}, + {"MAV_STRAIN_GAUGE_ID", 14}, }; 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 } + {"MAV_CMD_ARM", 1}, + {"MAV_CMD_DISARM", 2}, + {"MAV_CMD_CALIBRATE", 3}, + {"MAV_CMD_FORCE_INIT", 4}, + {"MAV_CMD_FORCE_LAUNCH", 5}, + {"MAV_CMD_FORCE_LANDING", 6}, + {"MAV_CMD_FORCE_EXPULSION", 7}, + {"MAV_CMD_FORCE_MAIN", 8}, + {"MAV_CMD_START_LOGGING", 9}, + {"MAV_CMD_CLOSE_LOG", 10}, + {"MAV_CMD_FORCE_REBOOT", 11}, + {"MAV_CMD_TEST_MODE", 12}, + {"MAV_CMD_START_RECORDING", 13}, + {"MAV_CMD_STOP_RECORDING", 14}, }; const QMap<QString, uint8_t> CommandPad::servosList = { - { "AIRBRAKES_SERVO", 1 }, - { "EXPULSION_SERVO", 2 }, - { "PARAFOIL_SERVO1", 3 }, - { "PARAFOIL_SERVO2", 4 }, + {"AIRBRAKES_SERVO", 1}, + {"EXPULSION_SERVO", 2}, + {"PARAFOIL_SERVO1", 3}, + {"PARAFOIL_SERVO2", 4}, }; - -CommandPad::CommandPad(QWidget* parent) : DefaultModule(parent) { +CommandPad::CommandPad(QWidget *parent) : DefaultModule(parent) { setupUi(); defaultContextMenuSetup(); } -CommandPad::~CommandPad() { - -} +CommandPad::~CommandPad() {} -QWidget* CommandPad::toWidget() { - return this; -} +QWidget *CommandPad::toWidget() { return this; } XmlObject CommandPad::toXmlObject() { - XmlObject obj(getName(ModuleId::COMMANDPAD)); - obj.addAttribute("selected", commandComboBox->currentText()); - return obj; + return XmlObject(getName(ModuleId::COMMANDPAD)); } -void CommandPad::fromXmlObject(const XmlObject& xmlObject) { - QString curr = xmlObject.getAttribute("selected"); - int idx = commandComboBox->findText(curr); - if(idx != -1) { - commandComboBox->setCurrentIndex(idx); - } +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)); \ +#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; \ +#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##_##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()); \ +#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(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()); + message.setField(#idProperty, id##_##idProperty->text().toDouble()); #define _SEND_UINT8_PROP(id, idProperty) \ - message.setField(#idProperty, (uint64_t) id##_##idProperty->text().toInt()); + 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()]); + message.setField(#idProperty, \ + (uint64_t)map[id##_##idProperty->currentText()]); -#define _SEND_END \ - getCore()->getModuleMessagesBroker()->publish(message); \ +#define _SEND_END \ + getCore()->getModuleMessagesBroker()->publish(message); \ } void CommandPad::setupUi() { - QStackedWidget* stacked = new QStackedWidget; + QStackedWidget *stacked = new QStackedWidget; - commandComboBox = new QComboBox; - commandComboBox->setSizePolicy(QSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed)); - connect(commandComboBox, QOverload<int>::of(&QComboBox::currentIndexChanged), this, - [ = ](int idx) { - stacked->setCurrentIndex(idx); - }); + 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; + QFormLayout *form = new QFormLayout; form->addRow(tr("Select command: "), commandComboBox); _CMD(ping, "PING_TC") @@ -198,8 +187,8 @@ void CommandPad::setupUi() { _CMD_UINT8_PROP(algorithm, algorithm_number) _CMD_END - QPushButton* send = new QPushButton(tr("Send")); - connect(send, &QPushButton::clicked, [ = ]() { + QPushButton *send = new QPushButton(tr("Send")); + connect(send, &QPushButton::clicked, [=]() { _SEND(ping, "PING_TC") _SEND_END _SEND(command, "COMMAND_TC") @@ -255,7 +244,7 @@ void CommandPad::setupUi() { _SEND_END }); - QVBoxLayout* layout = new QVBoxLayout; + QVBoxLayout *layout = new QVBoxLayout; layout->addLayout(form); layout->addWidget(stacked); layout->addWidget(send); diff --git a/Modules/Mavlink/mavlink_skyward_lib b/Modules/Mavlink/mavlink_skyward_lib index be1b8d4f61c546548d8b0c7d128dd304c9c49476..a5c41cbbd55adb8386dca873a9371e8895f26c09 160000 --- a/Modules/Mavlink/mavlink_skyward_lib +++ b/Modules/Mavlink/mavlink_skyward_lib @@ -1 +1 @@ -Subproject commit be1b8d4f61c546548d8b0c7d128dd304c9c49476 +Subproject commit a5c41cbbd55adb8386dca873a9371e8895f26c09 diff --git a/Modules/Mavlink/mavlinkcommandadapter.cpp b/Modules/Mavlink/mavlinkcommandadapter.cpp index 3fe0cdc6eb2a2c6da8c44365e40be6d643d619c4..bfa6caefe25dc5f82cd9ca21e59f8037cf889f22 100644 --- a/Modules/Mavlink/mavlinkcommandadapter.cpp +++ b/Modules/Mavlink/mavlinkcommandadapter.cpp @@ -3,69 +3,97 @@ #include "Core/modulesmanager.h" #include "Core/xmlobject.h" #include "Modules/skywardhubstrings.h" -#include "mavlinkversionheader.h" #include "mavlinkmodule.h" +#include "mavlinkversionheader.h" -MavlinkCommandAdapter::MavlinkCommandAdapter() { - -} +MavlinkCommandAdapter::MavlinkCommandAdapter() {} -void MavlinkCommandAdapter::setSerialPort(QSerialPort* value) { +void MavlinkCommandAdapter::setSerialPort(QSerialPort *value) { serial = value; } -bool MavlinkCommandAdapter::encodeCommand(const ModuleMessage& msg, mavlink_message_t& output) { - QString messageName = msg.getTopic().toString().replace(SkywardHubStrings::commandsTopic + "/", ""); - if(messageName == "PING_TC") { - mavlink_msg_ping_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("timestamp").getUInteger(0)); - return true; - } else if(messageName == "COMMAND_TC") { - mavlink_msg_command_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("command_id").getUInteger(0)); - return true; - } else if(messageName == "SYSTEM_TM_REQUEST_TC") { - mavlink_msg_system_tm_request_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("tm_id").getUInteger(0)); - return true; - } else if(messageName == "SENSOR_TM_REQUEST_TC") { - mavlink_msg_sensor_tm_request_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("sensor_id").getUInteger(0)); - return true; - } else if(messageName == "SERVO_TM_REQUEST_TC") { - mavlink_msg_servo_tm_request_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("servo_id").getUInteger(0)); - return true; - } else if(messageName == "SET_SERVO_ANGLE_TC") { - mavlink_msg_set_servo_angle_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("servo_id").getUInteger(0), msg.getField("angle").getDouble(0.0)); - return true; - } else if(messageName == "WIGGLE_SERVO_TC") { - mavlink_msg_wiggle_servo_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("servo_id").getUInteger(0)); - return true; - } else if(messageName == "RESET_SERVO_TC") { - mavlink_msg_reset_servo_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("servo_id").getUInteger(0)); - return true; - } else if(messageName == "SET_REFERENCE_ALTITUDE_TC") { - mavlink_msg_set_reference_altitude_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("ref_altitude").getDouble(0.0)); - return true; - } else if(messageName == "SET_REFERENCE_TEMPERATURE_TC") { - mavlink_msg_set_reference_temperature_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("ref_temp").getDouble(0.0)); - return true; - } else if(messageName == "SET_DEPLOYMENT_ALTITUDE_TC") { - mavlink_msg_set_deployment_altitude_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("dpl_altitude").getDouble(0.0)); - return true; - } else if(messageName == "SET_INITIAL_ORIENTATION_TC") { - mavlink_msg_set_initial_orientation_tc_pack(MAV_SYS, MAV_CMP, &output, - msg.getField("yaw").getDouble(0.0), - msg.getField("pitch").getDouble(0.0), - msg.getField("roll").getDouble(0.0)); - return true; - } else if(messageName == "SET_INITIAL_COORDINATES_TC") { - mavlink_msg_set_initial_coordinates_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("latitude").getDouble(0.0), msg.getField("longitude").getDouble(0.0)); - return true; - } else if(messageName == "RAW_EVENT_TC") { - mavlink_msg_raw_event_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("topic_id").getUInteger(0), msg.getField("event_id").getUInteger(0)); - return true; - } else if(messageName == "SET_TARGET_COORDINATES_TC") { - mavlink_msg_set_target_coordinates_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("latitude").getDouble(0.0), msg.getField("longitude").getDouble(0.0)); - return true; - } else if(messageName == "SET_ALGORITHM_TC") { - mavlink_msg_set_algorithm_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("algorithm_number").getUInteger(0)); +bool MavlinkCommandAdapter::encodeCommand(const ModuleMessage &msg, + mavlink_message_t &output) { + QString messageName = msg.getTopic().toString().replace( + SkywardHubStrings::commandsTopic + "/", ""); + if (messageName == "PING_TC") { + mavlink_msg_ping_tc_pack(MAV_SYS, MAV_CMP, &output, + msg.getField("timestamp").getUInteger(0)); + return true; + } else if (messageName == "COMMAND_TC") { + mavlink_msg_command_tc_pack(MAV_SYS, MAV_CMP, &output, + msg.getField("command_id").getUInteger(0)); + return true; + } else if (messageName == "SYSTEM_TM_REQUEST_TC") { + mavlink_msg_system_tm_request_tc_pack( + MAV_SYS, MAV_CMP, &output, msg.getField("tm_id").getUInteger(0)); + return true; + } else if (messageName == "SENSOR_TM_REQUEST_TC") { + mavlink_msg_sensor_tm_request_tc_pack( + MAV_SYS, MAV_CMP, &output, + msg.getField("sensor_id").getUInteger(0)); + return true; + } else if (messageName == "SERVO_TM_REQUEST_TC") { + mavlink_msg_servo_tm_request_tc_pack( + MAV_SYS, MAV_CMP, &output, msg.getField("servo_id").getUInteger(0)); + return true; + } else if (messageName == "SET_SERVO_ANGLE_TC") { + mavlink_msg_set_servo_angle_tc_pack( + MAV_SYS, MAV_CMP, &output, msg.getField("servo_id").getUInteger(0), + msg.getField("angle").getDouble(0.0)); + return true; + } else if (messageName == "SET_SERVO_ANGLE_TC") { + mavlink_msg_set_servo_angle_tc_pack( + MAV_SYS, MAV_CMP, &output, msg.getField("servo_id").getUInteger(0), + msg.getField("angle").getDouble(0.0)); + return true; + } else if (messageName == "WIGGLE_SERVO_TC") { + mavlink_msg_wiggle_servo_tc_pack( + MAV_SYS, MAV_CMP, &output, msg.getField("servo_id").getUInteger(0)); + return true; + } else if (messageName == "RESET_SERVO_TC") { + mavlink_msg_reset_servo_tc_pack( + MAV_SYS, MAV_CMP, &output, msg.getField("servo_id").getUInteger(0)); + return true; + } else if (messageName == "SET_REFERENCE_ALTITUDE_TC") { + mavlink_msg_set_reference_altitude_tc_pack( + MAV_SYS, MAV_CMP, &output, + msg.getField("ref_altitude").getDouble(0.0)); + return true; + } else if (messageName == "SET_REFERENCE_TEMPERATURE_TC") { + mavlink_msg_set_reference_temperature_tc_pack( + MAV_SYS, MAV_CMP, &output, msg.getField("ref_temp").getDouble(0.0)); + return true; + } else if (messageName == "SET_DEPLOYMENT_ALTITUDE_TC") { + mavlink_msg_set_deployment_altitude_tc_pack( + MAV_SYS, MAV_CMP, &output, + msg.getField("dpl_altitude").getDouble(0.0)); + return true; + } else if (messageName == "SET_INITIAL_ORIENTATION_TC") { + mavlink_msg_set_initial_orientation_tc_pack( + MAV_SYS, MAV_CMP, &output, msg.getField("yaw").getDouble(0.0), + msg.getField("pitch").getDouble(0.0), + msg.getField("roll").getDouble(0.0)); + return true; + } else if (messageName == "SET_INITIAL_COORDINATES_TC") { + mavlink_msg_set_initial_coordinates_tc_pack( + MAV_SYS, MAV_CMP, &output, msg.getField("latitude").getDouble(0.0), + msg.getField("longitude").getDouble(0.0)); + return true; + } else if (messageName == "RAW_EVENT_TC") { + mavlink_msg_raw_event_tc_pack(MAV_SYS, MAV_CMP, &output, + msg.getField("topic_id").getUInteger(0), + msg.getField("event_id").getUInteger(0)); + return true; + } else if (messageName == "SET_TARGET_COORDINATES_TC") { + mavlink_msg_set_target_coordinates_tc_pack( + MAV_SYS, MAV_CMP, &output, msg.getField("latitude").getDouble(0.0), + msg.getField("longitude").getDouble(0.0)); + return true; + } else if (messageName == "SET_ALGORITHM_TC") { + mavlink_msg_set_algorithm_tc_pack( + MAV_SYS, MAV_CMP, &output, + msg.getField("algorithm_number").getUInteger(0)); return true; } return false; @@ -76,9 +104,10 @@ void MavlinkCommandAdapter::send(mavlink_message_t msg) { mavlinkWriter.write(msg); } -bool MavlinkCommandAdapter::produceMsgFromXml(const XmlObject& xml, mavlink_message_t* msg) { +bool MavlinkCommandAdapter::produceMsgFromXml(const XmlObject &xml, + mavlink_message_t *msg) { bool result = false; - if(xml.getObjectName() == "ACK_TM") { + if (xml.getObjectName() == "ACK_TM") { QString recv_string = xml.getAttribute("recv_msgid"); QString seq_string = xml.getAttribute("seq_ack"); @@ -86,7 +115,7 @@ bool MavlinkCommandAdapter::produceMsgFromXml(const XmlObject& xml, mavlink_mess uint8_t recvId = recv_string.toUInt(&ok1); uint8_t seq = seq_string.toUInt(&ok2); - if(ok1 && ok2) { + if (ok1 && ok2) { mavlink_msg_ack_tm_pack(MAV_SYS, MAV_CMP, msg, recvId, seq); } result = true; @@ -94,29 +123,3 @@ bool MavlinkCommandAdapter::produceMsgFromXml(const XmlObject& xml, mavlink_mess return result; } - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Modules/Mavlink/mavlinkmodule.cpp b/Modules/Mavlink/mavlinkmodule.cpp index 793cf20d031ada64bb5595bfed970d582802b06a..82746d17816a3cda92abbd0776451ed976039afd 100644 --- a/Modules/Mavlink/mavlinkmodule.cpp +++ b/Modules/Mavlink/mavlinkmodule.cpp @@ -61,13 +61,13 @@ void MavlinkModule::initializeSerialPortView() { //fill the baud rate combo box QString baudPrefix = "BaudRate: "; ui->comboBox_baudRate->addItem(baudPrefix + "115200", QSerialPort::Baud115200); - ui->comboBox_baudRate->addItem(baudPrefix + "57600", QSerialPort::Baud57600); - ui->comboBox_baudRate->addItem(baudPrefix + "38400", QSerialPort::Baud38400); + // ui->comboBox_baudRate->addItem(baudPrefix + "57600", QSerialPort::Baud57600); + // ui->comboBox_baudRate->addItem(baudPrefix + "38400", QSerialPort::Baud38400); ui->comboBox_baudRate->addItem(baudPrefix + "19200", QSerialPort::Baud19200); ui->comboBox_baudRate->addItem(baudPrefix + "9600", QSerialPort::Baud9600); - ui->comboBox_baudRate->addItem(baudPrefix + "4800", QSerialPort::Baud4800); - ui->comboBox_baudRate->addItem(baudPrefix + "2400", QSerialPort::Baud2400); - ui->comboBox_baudRate->addItem(baudPrefix + "1200", QSerialPort::Baud1200); + // ui->comboBox_baudRate->addItem(baudPrefix + "4800", QSerialPort::Baud4800); + // ui->comboBox_baudRate->addItem(baudPrefix + "2400", QSerialPort::Baud2400); + // ui->comboBox_baudRate->addItem(baudPrefix + "1200", QSerialPort::Baud1200); bool first = true; @@ -148,13 +148,13 @@ void MavlinkModule::onLinkQualityIndicatorClicked(bool checked) { } void MavlinkModule::onLinkQualityTimerTick() { - float ratio = msgArrived / ui->spinBox_maxPackets->value(); + float ratio = (float)msgArrived / linkQualityPeriod * 1000.0; updateLinkSignalIndicator(msgArrived, ratio); msgArrived = 0; } void MavlinkModule::updateLinkSignalIndicator(int msgArrived, float ratio) { - ui->msgPerSecond_label->setText(QString::number(msgArrived)); + ui->msgPerSecond_label->setText(QString::number(ratio)); ModuleMessage linkQuality(SkywardHubStrings::mavlink_quality_link_topic); linkQuality.setField("ReceivedMsgNumber", {QString::number(msgArrived)}); linkQuality.setField("ReceivedRatio", {QString::number(ratio)}); diff --git a/Modules/Mavlink/mavlinkmodule.h b/Modules/Mavlink/mavlinkmodule.h index 35c122cd40d038249744c9797cef4510a3d2b99c..a37926e0675e6a94807cbedd5048d96df8182d75 100644 --- a/Modules/Mavlink/mavlinkmodule.h +++ b/Modules/Mavlink/mavlinkmodule.h @@ -1,47 +1,50 @@ #ifndef MAVLINKMODULE_H #define MAVLINKMODULE_H +#include <QSerialPort> +#include <QTimer> #include <QWidget> -#include "Core/module.h" + #include "Components/ToggleButton/togglebutton.h" -#include <QTimer> #include "Core/Message/modulemessage.h" -#include "mavlinkreader.h" -#include "mavlinkcommandadapter.h" -#include <QSerialPort> +#include "Core/module.h" #include "Modules/DefaultModule/defaultmodule.h" +#include "mavlinkcommandadapter.h" +#include "mavlinkreader.h" Q_DECLARE_METATYPE(ModuleMessage); -namespace Ui { -class MavlinkModule; +namespace Ui +{ + class MavlinkModule; } -class MavlinkModule : public DefaultModule { +class MavlinkModule : public DefaultModule +{ Q_OBJECT - public: - explicit MavlinkModule(QWidget* parent = nullptr); +public: + explicit MavlinkModule(QWidget *parent = nullptr); ~MavlinkModule(); - QWidget* toWidget() override; + QWidget *toWidget() override; XmlObject toXmlObject() override; - void fromXmlObject(const XmlObject& xmlObject) override; + void fromXmlObject(const XmlObject &xmlObject) override; - void onCommandReceived(const ModuleMessage& msg); + void onCommandReceived(const ModuleMessage &msg); - public slots: +public slots: void onStartClicked(); void onStopClicked(); - void onMsgReceived(const ModuleMessage& msg); + void onMsgReceived(const ModuleMessage &msg); void onLinkQualityIndicatorClicked(bool checked); void onLinkQualityTimerTick(); - protected: +protected: void connectUiSlots(); void initializeSerialPortView(); - mavlink_message_t parseMavlinkMsg(char* buffer, int readCount); + mavlink_message_t parseMavlinkMsg(char *buffer, int readCount); void closePort(); void updateLinkSignalIndicator(); @@ -49,13 +52,13 @@ class MavlinkModule : public DefaultModule { void updateLinkSignalIndicator(int msgArrived, float ratio); void onOpenLogFolderClick(); - private slots: +private slots: void onSerialPortIndexChanged(int index); bool startReadingOnSerialPort(); void onStartStreamToggled(bool state); - private: - Ui::MavlinkModule* ui; +private: + Ui::MavlinkModule *ui; ToggleButton startSerialStreamToggleButton; QString dateFormat = "HH:mm:ss\n(zzz)"; @@ -64,7 +67,7 @@ class MavlinkModule : public DefaultModule { QSerialPort serialPort; QTimer linkQualityTimer; - int linkQualityPeriod = 1000; // [ms] + int linkQualityPeriod = 2000; // [ms] int msgArrived = 0; bool portOpen; }; diff --git a/Modules/Mavlink/mavlinkreader.cpp b/Modules/Mavlink/mavlinkreader.cpp index b8bb38a2f35472cb58269b58536983546fbf43ee..d1790d5575fee48ca3d96cb2ea920ab1613fab5f 100644 --- a/Modules/Mavlink/mavlinkreader.cpp +++ b/Modules/Mavlink/mavlinkreader.cpp @@ -116,7 +116,16 @@ MessageField MavlinkReader::decodeField(const mavlink_message_t& msg, const mavl return decodeArrayElement(msg, field, 0); } else { if (field.type == MAVLINK_TYPE_CHAR) { - return MessageField(QString((const char*)_MAV_PAYLOAD(&msg))); + 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 MessageField(str); } else { QList<MessageField> array; array.reserve(field.array_length); diff --git a/Modules/Mavlink/mavlinkrocketmsgtestingmodule.cpp b/Modules/Mavlink/mavlinkrocketmsgtestingmodule.cpp index da4fb565924035ca6fc8b3c2462378daf0163427..3a4e9e22968862dfe7f6f559b5fb14a1c9796398 100644 --- a/Modules/Mavlink/mavlinkrocketmsgtestingmodule.cpp +++ b/Modules/Mavlink/mavlinkrocketmsgtestingmodule.cpp @@ -274,21 +274,3 @@ QString MsgView::getName() const { QMap<QString, QLineEdit*> MsgView::getFields() const { return fields; } - - - - - - - - - - - - - - - - - - diff --git a/Modules/Mavlink/mavlinkversionheader.h b/Modules/Mavlink/mavlinkversionheader.h index 05b00860606139a1dd732cf4018226a55d2c264a..466d8f770f947711e929782a041c717f9f0b9ba4 100644 --- a/Modules/Mavlink/mavlinkversionheader.h +++ b/Modules/Mavlink/mavlinkversionheader.h @@ -4,11 +4,10 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wcast-align" #pragma GCC diagnostic ignored "-Waddress-of-packed-member" -#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" #include "mavlink_skyward_lib/mavlink_lib/pyxis/mavlink.h" #pragma GCC diagnostic pop // static __attribute__((unused)) uint8_t validSysid = 171; // static __attribute__((unused)) uint8_t validCompid = 96; -#endif // MAVLINKVERSIONHEADER_H +#endif // MAVLINKVERSIONHEADER_H