From 4a313a37152c18db595ae5c976a853ade4d1b669 Mon Sep 17 00:00:00 2001 From: Pos <pierpaolo.mancini@mail.polimi.it> Date: Wed, 29 Sep 2021 20:56:16 +0200 Subject: [PATCH] Implemented mavlinkMsgOwnership check --- Modules/Mavlink/mavlink_skyward_lib | 2 +- Modules/Mavlink/mavlinkreader.cpp | 13 ++++++++++++- Modules/Mavlink/mavlinkreader.h | 4 ++++ SkywardHub.pro.user | 2 +- 4 files changed, 18 insertions(+), 3 deletions(-) diff --git a/Modules/Mavlink/mavlink_skyward_lib b/Modules/Mavlink/mavlink_skyward_lib index d7c4c017..5715deeb 160000 --- a/Modules/Mavlink/mavlink_skyward_lib +++ b/Modules/Mavlink/mavlink_skyward_lib @@ -1 +1 @@ -Subproject commit d7c4c0179f0cde99b3346181b51c6cd1a95747a8 +Subproject commit 5715deebff61d9443d2fdd68ceef03d35d8afc01 diff --git a/Modules/Mavlink/mavlinkreader.cpp b/Modules/Mavlink/mavlinkreader.cpp index 0d1ff150..733a9df9 100644 --- a/Modules/Mavlink/mavlinkreader.cpp +++ b/Modules/Mavlink/mavlinkreader.cpp @@ -53,12 +53,22 @@ void MavlinkReader::run() bool ok; while (!stop) { mavlink_message_t msg = waitForMavlinkMsg(ok); - if(ok){ + + if(ok && chackMsgOwnership(msg)){ parseMavlinkMsg(&msg); } } } +bool MavlinkReader::chackMsgOwnership(const mavlink_message_t& msg) const +{ + if(msg.sysid == validSysid && msg.compid == validCompid){ + return true; + } + return false; +} + + void MavlinkReader::setPortNumber(int value) { portNumber = value; @@ -168,6 +178,7 @@ void MavlinkReader::handleError() qDebug() << "MavlinkReader >> Serial read error"; } + QList<ModuleMessage> MavlinkReader::parseMavlinkMsg(mavlink_message_t *msg) { if(msg == nullptr) diff --git a/Modules/Mavlink/mavlinkreader.h b/Modules/Mavlink/mavlinkreader.h index 09d6a559..b768d1b1 100644 --- a/Modules/Mavlink/mavlinkreader.h +++ b/Modules/Mavlink/mavlinkreader.h @@ -45,6 +45,7 @@ protected: void handleReadyRead(); void handleError(); + bool chackMsgOwnership(const mavlink_message_t& msg) const; void setLogFilePath(); // QDateTime convertTimestampToDatatime(const double ×tamp) const; @@ -65,6 +66,9 @@ private: QString logFilePath = ""; QFile logFile; + + uint8_t validSysid = 171; + uint8_t validCompid = 96; }; #endif // MAVLINKREADER_H diff --git a/SkywardHub.pro.user b/SkywardHub.pro.user index d5bf3928..6899b2f2 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-09-12T23:02:44. --> +<!-- Written by QtCreator 4.14.2, 2021-09-29T20:55:13. --> <qtcreator> <data> <variable>EnvironmentId</variable> -- GitLab