diff --git a/Core/modulemessage.cpp b/Core/modulemessage.cpp index 88d8dfca25feb83c50f42d12eac1f2105b51ab33..1f938f08da869482b5bfadb0228f51d3460edf8e 100644 --- a/Core/modulemessage.cpp +++ b/Core/modulemessage.cpp @@ -129,6 +129,19 @@ bool ModuleMessage::getIntPayload(int &value) const return false; } +bool ModuleMessage::getFloatPayload(float &value) const +{ + bool ok; + if(payload() != ""){ + int temp = payload().toFloat(&ok); + if(ok){ + value = temp; + return true; + } + } + return false; +} + void ModuleMessage::copy(const ModuleMessage &msg) { setPayload(msg.payload()); diff --git a/Core/modulemessage.h b/Core/modulemessage.h index 8f54b4ab019c30b725b9cf62210a4df957c47b4b..bc9a8878cf9523a2366a15431178e99f588a7e17 100644 --- a/Core/modulemessage.h +++ b/Core/modulemessage.h @@ -43,10 +43,12 @@ public: void addOption(const QString &optionName, int value); /* - * Set the value of "value" if the payload exist and is an integer + * Get the value of "value" if the payload exist and is an integer */ bool getIntPayload(int &value) const; + bool getFloatPayload(float &value) const; + private: void copy(const ModuleMessage &msg); diff --git a/Core/xmlobject.cpp b/Core/xmlobject.cpp index d85f236718e527385aee98158e81e4e4e0ecfa0d..9a8ff19a8ea569be4fcbd21dbed086647d041bd1 100644 --- a/Core/xmlobject.cpp +++ b/Core/xmlobject.cpp @@ -244,6 +244,20 @@ bool XmlObject::getIntAttribute(const QString &name, int &value) const return false; } +bool XmlObject::getFloatAttribute(const QString &name, float &value) const +{ + QString val = getAttribute(name); + bool ok; + if(val != ""){ + int temp = val.toFloat(&ok); + if(ok){ + value = temp; + return true; + } + } + return false; +} + void XmlObject::addAttribute(const QString &name, const QString &value) { attributes[name] = value; diff --git a/Core/xmlobject.h b/Core/xmlobject.h index 0e48cac2f16adc0ccfc85e29bdd222473371591a..8b64516ce74c042d24e8f8b86144631e8e65e3d0 100644 --- a/Core/xmlobject.h +++ b/Core/xmlobject.h @@ -26,9 +26,10 @@ public: QMapIterator<QString, QString> attributesIterator() const; /* - * Set the value of "value" if the attribute with name "name" exist and is an integer + * Get the value of "value" if the attribute with name "name" exist and is an integer */ bool getIntAttribute(const QString &name, int &value) const; + bool getFloatAttribute(const QString &name, float &value) const; /* * addAttribute create a new attribute with name "name" and value "value" * If the attribute already exist, it change its value diff --git a/Modules/CommandPad/commandpadmodule.cpp b/Modules/CommandPad/commandpadmodule.cpp index e6b624e78dd9d6555b1ecfd6a370154f6e2a5a27..18045167386ddf5ebd0e0e14a3d3d1f8f01da245 100644 --- a/Modules/CommandPad/commandpadmodule.cpp +++ b/Modules/CommandPad/commandpadmodule.cpp @@ -40,16 +40,21 @@ void CommandPadModule::buildUI() connect(&logToggleButton, &ToggleButton::toggled, this, &CommandPadModule::onLogButtonToggled); connect(ui->closeLog_button, &QPushButton::clicked, this, &CommandPadModule::onCloseLogClicked); - connect(ui->wiggleServo_button, &QPushButton::clicked, this, &CommandPadModule::onWiggleServoClicked); + connect(ui->wiggleARBServo_button, &QPushButton::clicked, this, &CommandPadModule::onWiggleARBServoClicked); + connect(ui->resetARBServo_button, &QPushButton::clicked, this, &CommandPadModule::onResetARBServoClicked); + connect(ui->wiggleDPLServo_button, &QPushButton::clicked, this, &CommandPadModule::onWiggleDPLServoClicked); + connect(ui->resetDPLServo_button, &QPushButton::clicked, this, &CommandPadModule::onResetDPLServoClicked); connect(ui->testPrimaryCut_button, &QPushButton::clicked, this, &CommandPadModule::onTestPrimaryCutClicked); connect(ui->testMode_button, &QPushButton::clicked, this, &CommandPadModule::onTestModeClicked); - connect(ui->resetServo_button, &QPushButton::clicked, this, &CommandPadModule::onResetServoClicked); connect(ui->testBackupCut_button, &QPushButton::clicked, this, &CommandPadModule::onTestBackupCutClicked); connect(ui->boardReset_button, &QPushButton::clicked, this, &CommandPadModule::onBoardResetClicked); - connect(ui->calibrateAda_button, &QPushButton::clicked, this, &CommandPadModule::onCalibrateAdaClicked); + connect(ui->testAerobrakest_button, &QPushButton::clicked, this, &CommandPadModule::onTestAerobrakesClicked); + connect(ui->disableAerobrakes_button, &QPushButton::clicked, this, &CommandPadModule::onDisableAerobrakesClicked); connect(ui->deployAltitudeSet_button, &QPushButton::clicked, this, &CommandPadModule::onDeploymentAltitudeSetClicked); connect(ui->tempeatureSet_button, &QPushButton::clicked, this, &CommandPadModule::onRefTemperatureSetClicked); connect(ui->referenceAltitudeSet_button, &QPushButton::clicked, this, &CommandPadModule::onRefAltitudeSetClicked); + connect(ui->aerobrakeAngleSet_button, &QPushButton::clicked, this, &CommandPadModule::onAerobrakeAngleSetClicked); + connect(ui->initialOrientationSet_pushButton, &QPushButton::clicked, this, &CommandPadModule::onInitialOrientationSetClicked); connect(ui->arm_button, &QPushButton::clicked, this, &CommandPadModule::onArmClicked); connect(ui->disarm_button, &QPushButton::clicked, this, &CommandPadModule::onDisarmClicked); connect(ui->forceLaunch_button, &QPushButton::clicked, this, &CommandPadModule::onForceLaunchClicked); @@ -97,16 +102,14 @@ ModuleMessage CommandPadModule::createCommandMsg(const QString &msgKey) const return msg; } -ModuleMessage CommandPadModule::createUploadSettingsMsg(const QString &msgKey, int setting_value) const +ModuleMessage CommandPadModule::createCommandMsgWithPayload(const QString &msgKey, const QString &payload) const { ModuleMessage msg = createCommandMsg(msgKey); - -// msg.addOption("setting_id", setting_id); - msg.addOption(SkywardHubStrings::upload_settings_value, setting_value); - + msg.setPayload(payload); return msg; } + ModuleMessage CommandPadModule::createRawEventMsg(const QString &msgKey, int event_id, int topic_id) const { ModuleMessage msg = createCommandMsg(msgKey); @@ -140,9 +143,24 @@ void CommandPadModule::onCloseLogClicked() send(createCommandMsg("MAV_CMD_CLOSE_LOG")); } -void CommandPadModule::onWiggleServoClicked() +void CommandPadModule::onWiggleARBServoClicked() { - send(createCommandMsg("MAV_CMD_WIGGLE_SERVO")); + send(createCommandMsg("MAV_CMD_ARB_WIGGLE_SERVO")); +} + +void CommandPadModule::onResetARBServoClicked() +{ + send(createCommandMsg("MAV_CMD_ARB_RESET_SERVO")); +} + +void CommandPadModule::onWiggleDPLServoClicked() +{ + send(createCommandMsg("MAV_CMD_DPL_WIGGLE_SERVO")); +} + +void CommandPadModule::onResetDPLServoClicked() +{ + send(createCommandMsg("MAV_CMD_DPL_RESET_SERVO")); } void CommandPadModule::onTestPrimaryCutClicked() @@ -155,11 +173,6 @@ void CommandPadModule::onTestModeClicked() send(createCommandMsg("MAV_CMD_TEST_MODE")); } -void CommandPadModule::onResetServoClicked() -{ - send(createCommandMsg("MAV_CMD_RESET_SERVO")); -} - void CommandPadModule::onTestBackupCutClicked() { send(createCommandMsg("MAV_CMD_TEST_BACKUP_CUTTER")); @@ -170,27 +183,53 @@ void CommandPadModule::onBoardResetClicked() send(createCommandMsg("MAV_CMD_BOARD_RESET")); } -void CommandPadModule::onCalibrateAdaClicked() +void CommandPadModule::onTestAerobrakesClicked() { - send(createCommandMsg("MAV_CMD_CALIBRATE_ADA")); + send(createCommandMsg("MAV_CMD_TEST_AEROBRAKES")); +} + +void CommandPadModule::onDisableAerobrakesClicked() +{ + send(createCommandMsg("MAV_CMD_DISABLE_AEROBRAKES")); } void CommandPadModule::onDeploymentAltitudeSetClicked() { - int setting_value = ui->deploymentAltitude_spinBox->value(); - send(createUploadSettingsMsg("MAV_SET_DEPLOYMENT_ALTITUDE", setting_value)); + QString payload = QString::number(ui->deploymentAltitude_spinBox->value()); + send(createCommandMsgWithPayload("SET_DEPLOYMENT_ALTITUDE_TC", payload)); } void CommandPadModule::onRefTemperatureSetClicked() { - int setting_value = ui->refTemperature_spinBox->value(); - send(createUploadSettingsMsg("MAV_SET_REFERENCE_TEMP", setting_value)); + QString payload = QString::number(ui->refTemperature_spinBox->value()); + send(createCommandMsgWithPayload("SET_REFERENCE_TEMPERATURE_TC", payload)); } void CommandPadModule::onRefAltitudeSetClicked() +{ + QString payload = QString::number(ui->referenceAltitude_spinBox->value()); + send(createCommandMsgWithPayload("SET_REFERENCE_ALTITUDE", payload)); +} + +void CommandPadModule::onAerobrakeAngleSetClicked() { - int setting_value = ui->referenceAltitude_spinBox->value(); - send(createUploadSettingsMsg("MAV_SET_REFERENCE_ALTITUDE", setting_value)); + QString payload = QString::number(ui->aerobrakeAngle_spinBox->value()); + send(createCommandMsgWithPayload("SET_AEROBRAKE_ANGLE_TC", payload)); +} + +void CommandPadModule::onInitialOrientationSetClicked() +{ + ModuleMessage msg = createCommandMsg("SET_INITIAL_ORIENTATION_TC"); + + QString yaw = QString::number(ui->initialOrientationYaw_spinBox->value()); + QString pitch = QString::number(ui->initialOrientationPitch_spinBox->value()); + QString roll = QString::number(ui->initialOrientationRoll_spinBox->value()); + + msg.addOption(SkywardHubStrings::mavlink_orientation_tc_yaw_name, yaw); + msg.addOption(SkywardHubStrings::mavlink_orientation_tc_pitch_name, pitch); + msg.addOption(SkywardHubStrings::mavlink_orientation_tc_roll_name, roll); + + send(msg); } void CommandPadModule::onArmClicked() diff --git a/Modules/CommandPad/commandpadmodule.h b/Modules/CommandPad/commandpadmodule.h index fc46981490c68361bbea44ec974d4c52ef2ae15b..baba81965dd6525c3b5f27b8d370dcbdc7757a79 100644 --- a/Modules/CommandPad/commandpadmodule.h +++ b/Modules/CommandPad/commandpadmodule.h @@ -24,22 +24,27 @@ public: protected: void buildUI(); ModuleMessage createCommandMsg(const QString &msgKey) const; - ModuleMessage createUploadSettingsMsg(const QString &msgKey, int setting_value) const; + ModuleMessage createCommandMsgWithPayload(const QString &msgKey, const QString &payload) const; ModuleMessage createRawEventMsg(const QString &msgKey, int event_id, int topic_id) const; void send(const ModuleMessage &msg); void onLogButtonToggled(bool val); void onCloseLogClicked(); - void onWiggleServoClicked(); + void onWiggleARBServoClicked(); + void onResetARBServoClicked(); + void onWiggleDPLServoClicked(); + void onResetDPLServoClicked(); void onTestPrimaryCutClicked(); void onTestModeClicked(); - void onResetServoClicked(); void onTestBackupCutClicked(); void onBoardResetClicked(); - void onCalibrateAdaClicked(); + void onTestAerobrakesClicked(); + void onDisableAerobrakesClicked(); void onDeploymentAltitudeSetClicked(); void onRefTemperatureSetClicked(); void onRefAltitudeSetClicked(); + void onAerobrakeAngleSetClicked(); + void onInitialOrientationSetClicked(); void onArmClicked(); void onDisarmClicked(); void onForceLaunchClicked(); @@ -48,8 +53,6 @@ protected: void onEndMissionClicked(); void onRawEventSendClicked(); - - private: Ui::CommandPadModule *ui; diff --git a/Modules/CommandPad/commandpadmodule.ui b/Modules/CommandPad/commandpadmodule.ui index a3130014c64f5e7b7ad657f120d29306675f0a34..081f710102e9c59b9e9f107071c2ba949db8810e 100644 --- a/Modules/CommandPad/commandpadmodule.ui +++ b/Modules/CommandPad/commandpadmodule.ui @@ -6,14 +6,14 @@ <rect> <x>0</x> <y>0</y> - <width>369</width> - <height>669</height> + <width>338</width> + <height>883</height> </rect> </property> <property name="windowTitle"> <string>Form</string> </property> - <layout class="QVBoxLayout" name="verticalLayout_3" stretch="1,1,2,2,2,2,2"> + <layout class="QVBoxLayout" name="verticalLayout_3" stretch="1,1,0,0,2,2,2,2"> <item> <widget class="QGroupBox" name="groupBox"> <property name="title"> @@ -74,6 +74,9 @@ <property name="title"> <string>Log</string> </property> + <property name="alignment"> + <set>Qt::AlignCenter</set> + </property> <layout class="QHBoxLayout" name="horizontalLayout_4"> <item> <layout class="QHBoxLayout" name="toggle_button_layout"> @@ -114,46 +117,89 @@ <property name="title"> <string>Test</string> </property> + <property name="alignment"> + <set>Qt::AlignCenter</set> + </property> <layout class="QGridLayout" name="gridLayout"> - <item row="1" column="2"> - <widget class="QPushButton" name="boardReset_button"> + <item row="1" column="0"> + <widget class="QPushButton" name="wiggleDPLServo_button"> <property name="text"> - <string>BOARD RESET</string> + <string>WIGGLE DPL SERVO</string> </property> </widget> </item> - <item row="0" column="2"> - <widget class="QPushButton" name="testMode_button"> + <item row="2" column="2"> + <widget class="QPushButton" name="testBackupCut_button"> <property name="text"> - <string>TEST MODE</string> + <string>TEST BACKUP CUT</string> </property> </widget> </item> <item row="0" column="0"> - <widget class="QPushButton" name="wiggleServo_button"> + <widget class="QPushButton" name="wiggleARBServo_button"> <property name="text"> - <string>WIGGLE SERVO</string> + <string>WIGGLE ARB SERVO</string> </property> </widget> </item> - <item row="0" column="1"> + <item row="0" column="2"> <widget class="QPushButton" name="testPrimaryCut_button"> <property name="text"> <string>TEST PRIMARY CUT</string> </property> </widget> </item> - <item row="1" column="0"> - <widget class="QPushButton" name="resetServo_button"> + <item row="2" column="0"> + <widget class="QPushButton" name="resetARBServo_button"> <property name="text"> - <string>RESET SERVO</string> + <string>RESET ARB SERVO</string> </property> </widget> </item> - <item row="1" column="1"> - <widget class="QPushButton" name="testBackupCut_button"> + <item row="3" column="0"> + <widget class="QPushButton" name="resetDPLServo_button"> <property name="text"> - <string>TEST BACKUP CUT</string> + <string>RESET DPL SERVO</string> + </property> + </widget> + </item> + <item row="3" column="2"> + <widget class="QPushButton" name="boardReset_button"> + <property name="text"> + <string>BOARD RESET</string> + </property> + </widget> + </item> + <item row="1" column="2"> + <widget class="QPushButton" name="testMode_button"> + <property name="text"> + <string>TEST MODE</string> + </property> + </widget> + </item> + </layout> + </widget> + </item> + <item> + <widget class="QGroupBox" name="groupBox_5"> + <property name="title"> + <string>Aerobrakes</string> + </property> + <property name="alignment"> + <set>Qt::AlignCenter</set> + </property> + <layout class="QHBoxLayout" name="horizontalLayout_10"> + <item> + <widget class="QPushButton" name="testAerobrakest_button"> + <property name="text"> + <string>TEST</string> + </property> + </widget> + </item> + <item> + <widget class="QPushButton" name="disableAerobrakes_button"> + <property name="text"> + <string>Disable</string> </property> </widget> </item> @@ -175,13 +221,6 @@ <set>Qt::AlignCenter</set> </property> <layout class="QVBoxLayout" name="verticalLayout"> - <item> - <widget class="QPushButton" name="calibrateAda_button"> - <property name="text"> - <string>CALIBRATE ADA</string> - </property> - </widget> - </item> <item> <layout class="QHBoxLayout" name="horizontalLayout"> <item> @@ -296,6 +335,140 @@ </item> </layout> </item> + <item> + <layout class="QHBoxLayout" name="horizontalLayout_6"> + <item> + <widget class="QLabel" name="label_10"> + <property name="text"> + <string>Aerobrake Angle </string> + </property> + </widget> + </item> + <item> + <spacer name="horizontalSpacer_5"> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + <property name="sizeHint" stdset="0"> + <size> + <width>40</width> + <height>20</height> + </size> + </property> + </spacer> + </item> + <item> + <widget class="QSpinBox" name="aerobrakeAngle_spinBox"> + <property name="maximum"> + <number>99999</number> + </property> + </widget> + </item> + <item> + <widget class="QPushButton" name="aerobrakeAngleSet_button"> + <property name="text"> + <string>Set</string> + </property> + </widget> + </item> + </layout> + </item> + <item> + <layout class="QVBoxLayout" name="verticalLayout_4"> + <item> + <layout class="QHBoxLayout" name="horizontalLayout_9"> + <item> + <spacer name="horizontalSpacer_6"> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + <property name="sizeHint" stdset="0"> + <size> + <width>40</width> + <height>20</height> + </size> + </property> + </spacer> + </item> + <item> + <widget class="QPushButton" name="initialOrientationSet_pushButton"> + <property name="text"> + <string>Set initial orientation</string> + </property> + </widget> + </item> + <item> + <spacer name="horizontalSpacer_7"> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + <property name="sizeHint" stdset="0"> + <size> + <width>40</width> + <height>20</height> + </size> + </property> + </spacer> + </item> + </layout> + </item> + <item> + <layout class="QHBoxLayout" name="horizontalLayout_8"> + <item> + <widget class="QLabel" name="label_12"> + <property name="text"> + <string>Yaw</string> + </property> + <property name="alignment"> + <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set> + </property> + </widget> + </item> + <item> + <widget class="QSpinBox" name="initialOrientationYaw_spinBox"> + <property name="maximum"> + <number>9999</number> + </property> + </widget> + </item> + <item> + <widget class="QLabel" name="label_13"> + <property name="text"> + <string>Pitch</string> + </property> + <property name="alignment"> + <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set> + </property> + </widget> + </item> + <item> + <widget class="QSpinBox" name="initialOrientationPitch_spinBox"> + <property name="maximum"> + <number>9999</number> + </property> + </widget> + </item> + <item> + <widget class="QLabel" name="label_14"> + <property name="text"> + <string>Roll</string> + </property> + <property name="alignment"> + <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set> + </property> + </widget> + </item> + <item> + <widget class="QSpinBox" name="initialOrientationRoll_spinBox"> + <property name="maximum"> + <number>9999</number> + </property> + </widget> + </item> + </layout> + </item> + </layout> + </item> </layout> </widget> </item> @@ -304,6 +477,9 @@ <property name="title"> <string>Launch Control</string> </property> + <property name="alignment"> + <set>Qt::AlignCenter</set> + </property> <layout class="QGridLayout" name="gridLayout_4"> <item row="0" column="1"> <widget class="QPushButton" name="disarm_button"> @@ -341,6 +517,9 @@ <property name="title"> <string>Drogue</string> </property> + <property name="alignment"> + <set>Qt::AlignCenter</set> + </property> <layout class="QVBoxLayout" name="verticalLayout_2"> <item> <widget class="QPushButton" name="noseconeOpen_button"> @@ -364,6 +543,9 @@ <property name="title"> <string>Raw Event</string> </property> + <property name="alignment"> + <set>Qt::AlignCenter</set> + </property> <layout class="QHBoxLayout" name="horizontalLayout_7" stretch="0,0,0,0,0"> <item> <widget class="QLabel" name="label_9"> diff --git a/Modules/CommandPad/telemetryrequestmodule.cpp b/Modules/CommandPad/telemetryrequestmodule.cpp index de6db07ac9664c79fc9282f4dc2fff8b40e25ee8..0afc0cc97945a39890af3262b30545655bedf3aa 100644 --- a/Modules/CommandPad/telemetryrequestmodule.cpp +++ b/Modules/CommandPad/telemetryrequestmodule.cpp @@ -49,6 +49,9 @@ void TelemetryRequestModule::connectUI() connect(ui->hr_tm_pushButton, &QPushButton::clicked, this, &TelemetryRequestModule::on_HR_TM_Clicked); connect(ui->lr_tm_pushButton, &QPushButton::clicked, this, &TelemetryRequestModule::on_LR_TM_Clicked); connect(ui->test_tm_pushButton, &QPushButton::clicked, this, &TelemetryRequestModule::on_TEST_TM_Clicked); + + connect(ui->windtunnel_tm_pushButton, &QPushButton::clicked, this, &TelemetryRequestModule::on_WINDTUNNEL_TM_Clicked); + connect(ui->sensors_tm_pushButton, &QPushButton::clicked, this, &TelemetryRequestModule::on_SENSORS_TM_Clicked); } void TelemetryRequestModule::send(const ModuleMessage &msg) @@ -142,3 +145,15 @@ void TelemetryRequestModule::on_TEST_TM_Clicked() { send(createTmMessage("MAV_TEST_TM_ID")); } + +void TelemetryRequestModule::on_WINDTUNNEL_TM_Clicked() +{ + send(createTmMessage("MAV_WINDTUNNEL_TM_ID")); +} + +void TelemetryRequestModule::on_SENSORS_TM_Clicked() +{ + send(createTmMessage("MAV_SENSORS_TM_ID")); +} + + diff --git a/Modules/CommandPad/telemetryrequestmodule.h b/Modules/CommandPad/telemetryrequestmodule.h index 09649f5332137c48866f68a2110f5d0b93892454..a6e004ab1a10082a9e94883808e689ee633ca3dc 100644 --- a/Modules/CommandPad/telemetryrequestmodule.h +++ b/Modules/CommandPad/telemetryrequestmodule.h @@ -44,6 +44,9 @@ protected: void on_LR_TM_Clicked(); void on_TEST_TM_Clicked(); + void on_WINDTUNNEL_TM_Clicked(); + void on_SENSORS_TM_Clicked(); + private: Ui::TelemetryRequestModule *ui; }; diff --git a/Modules/CommandPad/telemetryrequestmodule.ui b/Modules/CommandPad/telemetryrequestmodule.ui index 59782424ad68d7a0ccd8b9d5eb85c7380a0dba88..50a8db9779df6f649682da5f88448c45e320f495 100644 --- a/Modules/CommandPad/telemetryrequestmodule.ui +++ b/Modules/CommandPad/telemetryrequestmodule.ui @@ -7,7 +7,7 @@ <x>0</x> <y>0</y> <width>245</width> - <height>509</height> + <height>521</height> </rect> </property> <property name="windowTitle"> @@ -19,6 +19,9 @@ <property name="title"> <string>Telemetry Request</string> </property> + <property name="alignment"> + <set>Qt::AlignCenter</set> + </property> <layout class="QVBoxLayout" name="verticalLayout_2"> <item> <widget class="QPushButton" name="sys_tm_pushButton"> @@ -132,6 +135,20 @@ </property> </widget> </item> + <item> + <widget class="QPushButton" name="windtunnel_tm_pushButton"> + <property name="text"> + <string>WIND-TUNNEL</string> + </property> + </widget> + </item> + <item> + <widget class="QPushButton" name="sensors_tm_pushButton"> + <property name="text"> + <string>SENSORS</string> + </property> + </widget> + </item> </layout> </widget> </item> diff --git a/Modules/Graph/graphmodule.cpp b/Modules/Graph/graphmodule.cpp index be91dd27beb31824a51a9eeca8e14e72257ea4e2..b47ed636a54e116d7d3419ac8884b3cd701aa174 100644 --- a/Modules/Graph/graphmodule.cpp +++ b/Modules/Graph/graphmodule.cpp @@ -172,8 +172,12 @@ void GraphModule::onSubscriptionRemoved(const QString &subscription) void GraphModule::onMsgReceived(const ModuleMessage &msg) { - if(subscriptions.contains(msg.topic())){ - QCPGraph *graph = subscriptions[msg.topic()]; + if(!subscriptions.contains(msg.originalTopic())){ + onSubscriptionAdded(msg.originalTopic()); + } + + if(subscriptions.contains(msg.originalTopic())){ + QCPGraph *graph = subscriptions[msg.originalTopic()]; double x = 0,y = 0; x = msg.timestamp().toTime_t(); diff --git a/Modules/Mavlink/mavlink_skyward_lib b/Modules/Mavlink/mavlink_skyward_lib index 00b5e98f28a1476587974dc623377af42883e6f0..f0b4285ffc1f8176a4a2f15f512a8476ad779aac 160000 --- a/Modules/Mavlink/mavlink_skyward_lib +++ b/Modules/Mavlink/mavlink_skyward_lib @@ -1 +1 @@ -Subproject commit 00b5e98f28a1476587974dc623377af42883e6f0 +Subproject commit f0b4285ffc1f8176a4a2f15f512a8476ad779aac diff --git a/Modules/Mavlink/mavlinkcommandadapter.cpp b/Modules/Mavlink/mavlinkcommandadapter.cpp index a92d8591f5b90b29a475fa6bb6ed3a5aa6eb6ab7..539374ab2baf0a0de0194aa2cd685781369d087e 100644 --- a/Modules/Mavlink/mavlinkcommandadapter.cpp +++ b/Modules/Mavlink/mavlinkcommandadapter.cpp @@ -50,17 +50,80 @@ mavlink_message_t MavlinkCommandAdapter::encode_TELEMETRY_REQUEST_TC(uint8_t boa return encoded_mvl_msg; } -mavlink_message_t MavlinkCommandAdapter::encode_RAW_EVENT_TC(uint8_t event_id, uint8_t topic_id) +mavlink_message_t MavlinkCommandAdapter::encode_RAW_EVENT_TC(const ModuleMessage &msg) { mavlink_message_t encoded_mvl_msg; - mavlink_msg_raw_event_tc_pack(MAV_SYS,MAV_CMP,&encoded_mvl_msg,event_id,topic_id); + uint8_t event_id, topic_id; + XmlObject msgOption = msg.getOptions(); + int temp; + if(msgOption.getIntAttribute(SkywardHubStrings::raw_event_id,temp)){ + event_id = temp; + if(msgOption.getIntAttribute(SkywardHubStrings::raw_event_topic_id,temp)){ + topic_id = temp; + mavlink_message_t encoded_mvl_msg; + mavlink_msg_raw_event_tc_pack(MAV_SYS,MAV_CMP,&encoded_mvl_msg,event_id,topic_id); + } + } + return encoded_mvl_msg; +} + +mavlink_message_t MavlinkCommandAdapter::encode_AEROBRAKE_ANGLE_TC(const ModuleMessage &msg) +{ + mavlink_message_t encoded_mvl_msg; + float angle; + + if(msg.getFloatPayload(angle)){ + mavlink_msg_set_aerobrake_angle_tc_pack(MAV_SYS,MAV_CMP,&encoded_mvl_msg, angle); + } + return encoded_mvl_msg; +} + +mavlink_message_t MavlinkCommandAdapter::encode_REFERENCE_ALTITUDE_TC(const ModuleMessage &msg) +{ + mavlink_message_t encoded_mvl_msg; + float ref_altitude; + + if(msg.getFloatPayload(ref_altitude)){ + mavlink_msg_set_reference_altitude_pack(MAV_SYS,MAV_CMP,&encoded_mvl_msg, ref_altitude); + } return encoded_mvl_msg; } -mavlink_message_t MavlinkCommandAdapter::encode_UPLOAD_SETTING_TC(uint8_t setting_id, float setting_value) +mavlink_message_t MavlinkCommandAdapter::encode_REFERENCE_TEMPERATURE_TC(const ModuleMessage &msg) { mavlink_message_t encoded_mvl_msg; - mavlink_msg_upload_setting_tc_pack(MAV_SYS,MAV_CMP, &encoded_mvl_msg, setting_id, setting_value); + float ref_temp; + + if(msg.getFloatPayload(ref_temp)){ + mavlink_msg_set_reference_temperature_tc_pack(MAV_SYS,MAV_CMP,&encoded_mvl_msg, ref_temp); + } + return encoded_mvl_msg; +} + +mavlink_message_t MavlinkCommandAdapter::encode_DEPLOYMENT_ALTITUDE_TC(const ModuleMessage &msg) +{ + mavlink_message_t encoded_mvl_msg; + float ref_altitude; + + if(msg.getFloatPayload(ref_altitude)){ + mavlink_msg_set_deployment_altitude_tc_pack(MAV_SYS,MAV_CMP,&encoded_mvl_msg, ref_altitude); + } + return encoded_mvl_msg; +} + +mavlink_message_t MavlinkCommandAdapter::encode_INITIAL_ORIENTATION_TC(const ModuleMessage &msg) +{ + mavlink_message_t encoded_mvl_msg; + float yaw, pitch, roll; + XmlObject msgOption = msg.getOptions(); + + bool ok = msgOption.getFloatAttribute(SkywardHubStrings::mavlink_orientation_tc_yaw_name, yaw); + ok = ok && msgOption.getFloatAttribute(SkywardHubStrings::mavlink_orientation_tc_pitch_name, pitch); + ok = ok && msgOption.getFloatAttribute(SkywardHubStrings::mavlink_orientation_tc_roll_name, roll); + + if(ok){ + mavlink_msg_set_initial_orientation_tc_pack(MAV_SYS,MAV_CMP,&encoded_mvl_msg, yaw, pitch, roll); + } return encoded_mvl_msg; } diff --git a/Modules/Mavlink/mavlinkcommandadapter.h b/Modules/Mavlink/mavlinkcommandadapter.h index d271ae579ecd97c1cc937ceab584fa60e3229dc7..ffd20cd2f4fb2f65addf4db46a17b5af849788c5 100644 --- a/Modules/Mavlink/mavlinkcommandadapter.h +++ b/Modules/Mavlink/mavlinkcommandadapter.h @@ -27,8 +27,12 @@ public: mavlink_message_t encode_NOARG_TC(int cmd_id); mavlink_message_t encode_START_LAUNCH_TC(const ModuleMessage &msg); mavlink_message_t encode_TELEMETRY_REQUEST_TC(uint8_t board_id); - 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); + mavlink_message_t encode_RAW_EVENT_TC(const ModuleMessage &msg); + mavlink_message_t encode_AEROBRAKE_ANGLE_TC(const ModuleMessage &msg); + mavlink_message_t encode_REFERENCE_ALTITUDE_TC(const ModuleMessage &msg); + mavlink_message_t encode_REFERENCE_TEMPERATURE_TC(const ModuleMessage &msg); + mavlink_message_t encode_DEPLOYMENT_ALTITUDE_TC(const ModuleMessage &msg); + mavlink_message_t encode_INITIAL_ORIENTATION_TC(const ModuleMessage &msg); bool produceMsgFromXml(const XmlObject &xml, mavlink_message_t *msg); diff --git a/Modules/Mavlink/mavlinkmodule.cpp b/Modules/Mavlink/mavlinkmodule.cpp index 0a6b3733d2669f4eee816e620b83c6a646fcfe76..e09071a9b798292f62e6f0b29340b0bc722207ad 100644 --- a/Modules/Mavlink/mavlinkmodule.cpp +++ b/Modules/Mavlink/mavlinkmodule.cpp @@ -57,45 +57,6 @@ void MavlinkModule::subscribe() getCore()->getModuleMessagesBroker()->subscribe(SkywardHubStrings::telemetryRequestTopic,this,[this](const ModuleMessage &msg){ onTelemetryRequestReceived(msg); }); - -// QString PING_TC_topic = prefix + QVariant::fromValue(MavlinkCommandAdapter::PING_TC).toString(); -// QString NOARG_TC_topic = prefix + QVariant::fromValue(MavlinkCommandAdapter::NOARG_TC).toString(); -// QString START_LAUNCH_TC_topic = prefix + QVariant::fromValue(MavlinkCommandAdapter::START_LAUNCH_TC).toString(); -// QString TELEMETRY_REQUEST_TC_topic = prefix + QVariant::fromValue(MavlinkCommandAdapter::TELEMETRY_REQUEST_TC).toString(); -// QString RAW_EVENT_TC_topic = prefix + QVariant::fromValue(MavlinkCommandAdapter::RAW_EVENT_TC).toString(); -// QString UPLOAD_SETTING_TC_topic = prefix + QVariant::fromValue(MavlinkCommandAdapter::UPLOAD_SETTING_TC).toString(); - -// QString PING_TC_topic = prefix + "PING_TC"; -// QString NOARG_TC_topic = prefix + MavlinkCommandAdapter::getStringId(MavlinkCommandAdapter::NOARG_TC); -// QString START_LAUNCH_TC_topic = prefix + MavlinkCommandAdapter::getStringId(MavlinkCommandAdapter::START_LAUNCH_TC); -// QString TELEMETRY_REQUEST_TC_topic = prefix + MavlinkCommandAdapter::getStringId(MavlinkCommandAdapter::TELEMETRY_REQUEST_TC); -// QString RAW_EVENT_TC_topic = prefix + MavlinkCommandAdapter::getStringId(MavlinkCommandAdapter::RAW_EVENT_TC); -// QString UPLOAD_SETTING_TC_topic = prefix + MavlinkCommandAdapter::getStringId(MavlinkCommandAdapter::UPLOAD_SETTING_TC); - - -// getCore()->getModuleMessagesBroker()->subscribe(PING_TC_topic, this, [this](const ModuleMessage &msg){ -// mavlinkCommandAdapter.send(mavlinkCommandAdapter.encode_PING_TC(msg)); -// }); - -// getCore()->getModuleMessagesBroker()->subscribe(NOARG_TC_topic, this, [this](const ModuleMessage &msg){ -// mavlinkCommandAdapter.send(mavlinkCommandAdapter.encode_NOARG_TC(msg)); -// }); - -// getCore()->getModuleMessagesBroker()->subscribe(START_LAUNCH_TC_topic, this, [this](const ModuleMessage &msg){ -// mavlinkCommandAdapter.send(mavlinkCommandAdapter.encode_START_LAUNCH_TC(msg)); -// }); - -// getCore()->getModuleMessagesBroker()->subscribe(TELEMETRY_REQUEST_TC_topic, this, [this](const ModuleMessage &msg){ -// mavlinkCommandAdapter.send(mavlinkCommandAdapter.encode_TELEMETRY_REQUEST_TC(msg)); -// }); - -// getCore()->getModuleMessagesBroker()->subscribe(RAW_EVENT_TC_topic, this, [this](const ModuleMessage &msg){ -// mavlinkCommandAdapter.send(mavlinkCommandAdapter.encode_RAW_EVENT_TC(msg)); -// }); - -// getCore()->getModuleMessagesBroker()->subscribe(UPLOAD_SETTING_TC_topic, this, [this](const ModuleMessage &msg){ -// mavlinkCommandAdapter.send(mavlinkCommandAdapter.encode_UPLOAD_SETTING_TC(msg)); -// }); } void MavlinkModule::publish(const ModuleMessage &msg) @@ -223,36 +184,31 @@ void MavlinkModule::onCommandReceived(const ModuleMessage &msg) if(arg == "PING_TC"){ encoded_mvl_msg = mavlinkCommandAdapter.encode_PING_TC(); } - else if(arg == "MAV_CMD_START_LOGGING"){ - encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_START_LOGGING); - } - else if(arg == "MAV_CMD_STOP_LOGGING"){ - encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_STOP_LOGGING); - } - else if(arg == "MAV_CMD_CLOSE_LOG"){ - encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_CLOSE_LOG); - } - else if(arg == "MAV_CMD_WIGGLE_SERVO"){ - encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_WIGGLE_SERVO); + else if(arg == "START_LAUNCH_TC"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_START_LAUNCH_TC(msg); } - else if(arg == "MAV_CMD_TEST_PRIMARY_CUTTER"){ - encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_TEST_PRIMARY_CUTTER); + + else if(arg == "SET_AEROBRAKE_ANGLE_TC"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_AEROBRAKE_ANGLE_TC(msg); } - else if(arg == "MAV_CMD_TEST_MODE"){ - encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_TEST_MODE); + else if(arg == "SET_REFERENCE_ALTITUDE"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_REFERENCE_ALTITUDE_TC(msg); } - else if(arg == "MAV_CMD_RESET_SERVO"){ - encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_RESET_SERVO); + else if(arg == "SET_REFERENCE_TEMPERATURE_TC"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_REFERENCE_TEMPERATURE_TC(msg); } - else if(arg == "MAV_CMD_TEST_BACKUP_CUTTER"){ - encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_TEST_BACKUP_CUTTER); + else if(arg == "SET_DEPLOYMENT_ALTITUDE_TC"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_DEPLOYMENT_ALTITUDE_TC(msg); } - else if(arg == "MAV_CMD_BOARD_RESET"){ - encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_BOARD_RESET); + else if(arg == "SET_INITIAL_ORIENTATION_TC"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_INITIAL_ORIENTATION_TC(msg); } - else if(arg == "MAV_CMD_CALIBRATE_ADA"){ - encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_CALIBRATE_ADA); + else if(arg == "RAW_EVENT_TC"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_RAW_EVENT_TC(msg); } + + + // NOARG_TC else if(arg == "MAV_CMD_ARM"){ encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_ARM); } @@ -262,54 +218,106 @@ void MavlinkModule::onCommandReceived(const ModuleMessage &msg) else if(arg == "MAV_CMD_FORCE_LAUNCH"){ encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_FORCE_LAUNCH); } + else if(arg == "MAV_CMD_CALIBRATE_SENSORS"){ // New Lynx + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_CALIBRATE_SENSORS); + } + else if(arg == "MAV_CMD_CALIBRATE_ALGOS"){ // New Lynx + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_CALIBRATE_ALGOS); + } else if(arg == "MAV_CMD_FORCE_INIT"){ encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_FORCE_INIT); } else if(arg == "MAV_CMD_NOSECONE_OPEN"){ encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_NOSECONE_OPEN); } - else if(arg == "MAV_CMD_END_MISSION"){ - encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_END_MISSION); + else if(arg == "MAV_CMD_DPL_WIGGLE_SERVO"){ // New Lynx + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_DPL_WIGGLE_SERVO); } - else if(arg == "MAV_SET_DEPLOYMENT_ALTITUDE"){ - float setting_value; - XmlObject msgOption = msg.getOptions(); - int temp; - if(msgOption.getIntAttribute(SkywardHubStrings::upload_settings_value, temp)){ - setting_value = temp; - encoded_mvl_msg = mavlinkCommandAdapter.encode_UPLOAD_SETTING_TC(MAV_SET_DEPLOYMENT_ALTITUDE, setting_value); - } + else if(arg == "MAV_CMD_ARB_WIGGLE_SERVO"){ // New Lynx + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_ARB_WIGGLE_SERVO); } - else if(arg == "MAV_SET_REFERENCE_TEMP"){ - float setting_value; - XmlObject msgOption = msg.getOptions(); - int temp; - if(msgOption.getIntAttribute(SkywardHubStrings::upload_settings_value, temp)){ - setting_value = temp; - encoded_mvl_msg = mavlinkCommandAdapter.encode_UPLOAD_SETTING_TC(MAV_SET_REFERENCE_TEMP, setting_value); - } + else if(arg == "MAV_CMD_DPL_RESET_SERVO"){ // New Lynx + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_DPL_RESET_SERVO); } - else if(arg == "MAV_SET_REFERENCE_ALTITUDE"){ - float setting_value; - XmlObject msgOption = msg.getOptions(); - int temp; - if(msgOption.getIntAttribute(SkywardHubStrings::upload_settings_value, temp)){ - setting_value = temp; - encoded_mvl_msg = mavlinkCommandAdapter.encode_UPLOAD_SETTING_TC(MAV_SET_REFERENCE_ALTITUDE, setting_value); - } + else if(arg == "MAV_CMD_ARB_RESET_SERVO"){ // New Lynx + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_ARB_RESET_SERVO); } - else if(arg == "RAW_EVENT_TC"){ - uint8_t Event_id, Topic_id; - XmlObject msgOption = msg.getOptions(); - int temp; - if(msgOption.getIntAttribute(SkywardHubStrings::raw_event_id,temp)){ - Event_id = temp; - if(msgOption.getIntAttribute(SkywardHubStrings::raw_event_topic_id,temp)){ - Topic_id = temp; - encoded_mvl_msg = mavlinkCommandAdapter.encode_RAW_EVENT_TC(Event_id, Topic_id); - } - } + else if(arg == "MAV_CMD_CUT_DROGUE"){ // New Lynx + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_CUT_DROGUE); + } + else if(arg == "MAV_CMD_CUT_PRIMARY"){ // New Lynx + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_CUT_PRIMARY); + } + else if(arg == "MAV_CMD_CUT_BACKUP"){ // New Lynx + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_CUT_BACKUP); + } + else if(arg == "MAV_CMD_TEST_PRIMARY_CUTTER"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_TEST_PRIMARY_CUTTER); + } + else if(arg == "MAV_CMD_TEST_BACKUP_CUTTER"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_TEST_BACKUP_CUTTER); + } + else if(arg == "MAV_CMD_START_LOGGING"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_START_LOGGING); + } + else if(arg == "MAV_CMD_STOP_LOGGING"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_STOP_LOGGING); + } + else if(arg == "MAV_CMD_CLOSE_LOG"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_CLOSE_LOG); + } + else if(arg == "MAV_CMD_TEST_AEROBRAKES"){ // New Lynx + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_TEST_AEROBRAKES); + } + else if(arg == "MAV_CMD_DISABLE_AEROBRAKES"){ // New Lynx + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_DISABLE_AEROBRAKES); + } + else if(arg == "MAV_CMD_END_MISSION"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_END_MISSION); + } + else if(arg == "MAV_CMD_BOARD_RESET"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_BOARD_RESET); } + else if(arg == "MAV_CMD_TEST_MODE"){ + encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_TEST_MODE); + } +// else if(arg == "MAV_CMD_WIGGLE_SERVO"){ +// encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_WIGGLE_SERVO); +// } +// else if(arg == "MAV_CMD_RESET_SERVO"){ +// encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_RESET_SERVO); +// } +// else if(arg == "MAV_CMD_CALIBRATE_ADA"){ +// encoded_mvl_msg = mavlinkCommandAdapter.encode_NOARG_TC(MAV_CMD_CALIBRATE_ADA); +// } + +// else if(arg == "MAV_SET_DEPLOYMENT_ALTITUDE"){ +// float setting_value; +// XmlObject msgOption = msg.getOptions(); +// int temp; +// if(msgOption.getIntAttribute(SkywardHubStrings::upload_settings_value, temp)){ +// setting_value = temp; +// encoded_mvl_msg = mavlinkCommandAdapter.encode_UPLOAD_SETTING_TC(MAV_SET_DEPLOYMENT_ALTITUDE_TC, setting_value); +// } +// } +// else if(arg == "MAV_SET_REFERENCE_TEMP"){ +// float setting_value; +// XmlObject msgOption = msg.getOptions(); +// int temp; +// if(msgOption.getIntAttribute(SkywardHubStrings::upload_settings_value, temp)){ +// setting_value = temp; +// encoded_mvl_msg = mavlinkCommandAdapter.encode_UPLOAD_SETTING_TC(MAV_SET_REFERENCE_TEMP, setting_value); +// } +// } +// else if(arg == "MAV_SET_REFERENCE_ALTITUDE"){ +// float setting_value; +// XmlObject msgOption = msg.getOptions(); +// int temp; +// if(msgOption.getIntAttribute(SkywardHubStrings::upload_settings_value, temp)){ +// setting_value = temp; +// encoded_mvl_msg = mavlinkCommandAdapter.encode_UPLOAD_SETTING_TC(MAV_SET_REFERENCE_ALTITUDE, setting_value); +// } +// } mavlinkCommandAdapter.send(encoded_mvl_msg); } @@ -366,6 +374,12 @@ void MavlinkModule::onTelemetryRequestReceived(const ModuleMessage &msg) else if(arg == "MAV_TEST_TM_ID"){ encoded_mvl_msg = mavlinkCommandAdapter.encode_TELEMETRY_REQUEST_TC(MAV_TEST_TM_ID); } + else if(arg == "MAV_WINDTUNNEL_TM_ID"){ // New Lynx + encoded_mvl_msg = mavlinkCommandAdapter.encode_TELEMETRY_REQUEST_TC(MAV_WINDTUNNEL_TM_ID); + } + else if(arg == "MAV_SENSORS_TM_ID"){ // New Lynx + encoded_mvl_msg = mavlinkCommandAdapter.encode_TELEMETRY_REQUEST_TC(MAV_SENSORS_TM_ID); + } mavlinkCommandAdapter.send(encoded_mvl_msg); } diff --git a/Modules/Mavlink/mavlinkreader.h b/Modules/Mavlink/mavlinkreader.h index 2bfeaac605eca80cb80eb9a31caae3d30a19cc79..9a43a237ccadf056a48756fd11d9c533e9cab090 100644 --- a/Modules/Mavlink/mavlinkreader.h +++ b/Modules/Mavlink/mavlinkreader.h @@ -5,7 +5,7 @@ #include <QThread> #include <QDateTime> #include "Core/xmlobject.h" -#include "mavlink_skyward_lib/mavlink_lib/hermes/mavlink.h" +#include "mavlinkversionheader.h" #include "Core/modulemessage.h" diff --git a/Modules/Mavlink/mavlinkrocketmsgtestingmodule.cpp b/Modules/Mavlink/mavlinkrocketmsgtestingmodule.cpp index 39138630d5eb9e3957687693141ee70dd9e970c1..2bd289145b674c075b13e6873d69e6d82ec65d24 100644 --- a/Modules/Mavlink/mavlinkrocketmsgtestingmodule.cpp +++ b/Modules/Mavlink/mavlinkrocketmsgtestingmodule.cpp @@ -172,7 +172,6 @@ void MavlinkRocketMsgTestingModule::onSendMsgClicked() if(currentMsgView != nullptr){ XmlObject xmlMsg = msgToXml(); testMavlinkEncodeAndDecode(xmlMsg); - } } diff --git a/Modules/Mavlink/mavlinkversionheader.h b/Modules/Mavlink/mavlinkversionheader.h index 771391c9f2591551a5097c97e5e5f20c564d91df..f4b4d0cf83f9c448ad94f39adc5bfc0bc14a88cd 100644 --- a/Modules/Mavlink/mavlinkversionheader.h +++ b/Modules/Mavlink/mavlinkversionheader.h @@ -1,6 +1,8 @@ #ifndef MAVLINKVERSIONHEADER_H #define MAVLINKVERSIONHEADER_H -#include "mavlink_skyward_lib/mavlink_lib/hermes/mavlink.h" +//#include "mavlink_skyward_lib/mavlink_lib/hermes/mavlink.h" +#include "mavlink_skyward_lib/mavlink_lib/lynx/mavlink.h" + #endif // MAVLINKVERSIONHEADER_H diff --git a/Modules/MessageViewer/messagesviewermodule.cpp b/Modules/MessageViewer/messagesviewermodule.cpp index c77d14020124bf7c6f47663d65eb0589b8e37ab8..320c2b08eeb01917081f1d6c16ecb63963723e72 100644 --- a/Modules/MessageViewer/messagesviewermodule.cpp +++ b/Modules/MessageViewer/messagesviewermodule.cpp @@ -44,13 +44,19 @@ void MessagesViewerModule::addMsgSent(const ModuleMessage &msg) int row = updateVerticalHeaders(msg); int column = 0; - QTableWidgetItem *newItem = new QTableWidgetItem(msg.payload()); + QString msgName = computeMsgName(msg); + QTableWidgetItem *newItem = new QTableWidgetItem(msgName); newItem->setBackground(QBrush(QColor(255, 153, 51))); newItem->setForeground(QBrush(QColor(0,0,0))); ui->tableWidget->setItem(row,column,newItem); registerMessage(newItem,msg); } +QString MessagesViewerModule::computeMsgName(const ModuleMessage &msg) +{ + return msg.originalTopic().replace(SkywardHubStrings::commandsTopic+"/", ""); +} + void MessagesViewerModule::handleAck(const ModuleMessage &ack) { for(int i=0; i < messages.count(); i++){ @@ -91,6 +97,8 @@ void MessagesViewerModule::registerMessage(QTableWidgetItem *item, const ModuleM } } + + // ____________________ MESSAGE LOG CLASS _______________________________ MessageLog::MessageLog(QTableWidgetItem *item, const ModuleMessage &msg) diff --git a/Modules/MessageViewer/messagesviewermodule.h b/Modules/MessageViewer/messagesviewermodule.h index abfbbe4946001568d3647ef7d5c383e5e504995a..e060e44e6022e390f581181076b75bd76f367209 100644 --- a/Modules/MessageViewer/messagesviewermodule.h +++ b/Modules/MessageViewer/messagesviewermodule.h @@ -48,6 +48,7 @@ public: protected: int updateVerticalHeaders(const ModuleMessage &msg); void registerMessage(QTableWidgetItem *item, const ModuleMessage &msg); + QString computeMsgName(const ModuleMessage &msg); private: Ui::MessagesViewerModule *ui; diff --git a/Modules/Table/tablemodule.cpp b/Modules/Table/tablemodule.cpp index de6ecd98d18b48fbaf2966a029f9ce0878662044..19afc6f51cd8a1d4c89aecf6e3186949e8d34b9d 100644 --- a/Modules/Table/tablemodule.cpp +++ b/Modules/Table/tablemodule.cpp @@ -93,7 +93,7 @@ void TableModule::removeRow(const QString &txt) void TableModule::onMsgReceived(const ModuleMessage &msg) { - int row = updateVerticalHeaders(msg.topic()); + int row = updateVerticalHeaders(msg.originalTopic()); int column = updateHorizontalHeaders(msg.timestamp().toString(horizontalHeaderDateFormat)); QTableWidgetItem *newItem = new QTableWidgetItem(msg.payload()); diff --git a/Modules/skywardhubstrings.cpp b/Modules/skywardhubstrings.cpp index 9c12891b46df4b288b9bdb7b64c601d4ffe0a3bf..fdc7f72dc879719636548486f04074b6b9e3060e 100644 --- a/Modules/skywardhubstrings.cpp +++ b/Modules/skywardhubstrings.cpp @@ -32,8 +32,9 @@ const QString SkywardHubStrings::treeViewerReceivingBaseTopic = "TreeViewer"; const QString SkywardHubStrings::logCommandsTopic = "LogCommands"; const QString SkywardHubStrings::raw_event_id = "event_id"; const QString SkywardHubStrings::raw_event_topic_id = "topic_id"; -const QString SkywardHubStrings::upload_settings_id = "settings_id"; -const QString SkywardHubStrings::upload_settings_value = "settings_value"; +const QString SkywardHubStrings::mavlink_orientation_tc_yaw_name = "yaw"; +const QString SkywardHubStrings::mavlink_orientation_tc_pitch_name = "pitch"; +const QString SkywardHubStrings::mavlink_orientation_tc_roll_name = "roll"; const QString SkywardHubStrings::msgIdField = "id"; const QString SkywardHubStrings::msgSequenceNumberField = "sequenceNumber"; diff --git a/Modules/skywardhubstrings.h b/Modules/skywardhubstrings.h index 9241e2b8e6b1c0db66b45845f09dfe02c63a5681..89acb85175e81bf7ec9b48c5ea300f534e51e718 100644 --- a/Modules/skywardhubstrings.h +++ b/Modules/skywardhubstrings.h @@ -38,8 +38,9 @@ public: static const QString logCommandsTopic; static const QString raw_event_id; static const QString raw_event_topic_id; - static const QString upload_settings_id; - static const QString upload_settings_value; + static const QString mavlink_orientation_tc_yaw_name; + static const QString mavlink_orientation_tc_pitch_name; + static const QString mavlink_orientation_tc_roll_name; static const QString msgIdField; static const QString msgSequenceNumberField; diff --git a/SkywardHub.pro.user b/SkywardHub.pro.user index 6ff847bea2578248a13044410f82927b661020ef..8ba22efbe12f1f6a1ec78726fe9a30168e01b9b0 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-05-17T20:57:56. --> +<!-- Written by QtCreator 4.14.2, 2021-05-25T15:26:01. --> <qtcreator> <data> <variable>EnvironmentId</variable>