From 7259a21c7f9399da854153d9f3bdf1b98427a9ae Mon Sep 17 00:00:00 2001 From: jnomikos Date: Mon, 3 Jul 2023 01:06:51 -0500 Subject: [PATCH 1/8] Operator id added to mavlink receiver --- msg/CMakeLists.txt | 1 + msg/OpenDroneIdOperatorId.msg | 6 ++++++ src/modules/mavlink/mavlink_receiver.cpp | 25 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 3 +++ 4 files changed, 35 insertions(+) create mode 100644 msg/OpenDroneIdOperatorId.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index f161085e64a4..3b251b7b3e21 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -138,6 +138,7 @@ set(msg_files OffboardControlMode.msg OnboardComputerStatus.msg OpenDroneIdArmStatus.msg + OpenDroneIdOperatorId.msg OrbitStatus.msg OrbTest.msg OrbTestLarge.msg diff --git a/msg/OpenDroneIdOperatorId.msg b/msg/OpenDroneIdOperatorId.msg new file mode 100644 index 000000000000..d75a419717f5 --- /dev/null +++ b/msg/OpenDroneIdOperatorId.msg @@ -0,0 +1,6 @@ +uint64 timestamp +uint8 target_system +uint8 target_component +uint8[20] id_or_mac +uint8 operator_id_type +char[20] operator_id diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 500339290161..eb73e41b8a0b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -288,6 +288,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_open_drone_id_arm_status(msg); break; + case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID: + handle_message_open_drone_id_operator_id(msg); + break; + #if !defined(CONSTRAINED_FLASH) case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: @@ -3088,6 +3092,27 @@ void MavlinkReceiver::handle_message_open_drone_id_arm_status( _open_drone_id_arm_status_pub.publish(odid_arm); } + +void MavlinkReceiver::handle_message_open_drone_id_operator_id( + mavlink_message_t *msg) +{ + + mavlink_open_drone_id_operator_id_t odid_module; + mavlink_msg_open_drone_id_operator_id_decode(msg, &odid_module); + + open_drone_id_operator_id_s odid_operator_id{}; + memset(&odid_operator_id, 0, sizeof(odid_operator_id)); + + odid_operator_id.timestamp = hrt_absolute_time(); + odid_operator_id.target_system = odid_module.target_system; + odid_operator_id.target_component = odid_module.target_component; + memcpy(odid_operator_id.id_or_mac, odid_module.id_or_mac, sizeof(odid_operator_id.id_or_mac)); + odid_operator_id.operator_id_type = odid_module.operator_id_type; + memcpy(odid_operator_id.operator_id, odid_module.operator_id, sizeof(odid_operator_id.operator_id)); + + _open_drone_id_operator_id_pub.publish(odid_operator_id); +} + void MavlinkReceiver::run() { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index d6ee5a976569..a924bec93bb9 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -88,6 +88,7 @@ #include #include #include +#include #include #include #include @@ -181,6 +182,7 @@ class MavlinkReceiver : public ModuleParams void handle_message_odometry(mavlink_message_t *msg); void handle_message_onboard_computer_status(mavlink_message_t *msg); void handle_message_open_drone_id_arm_status(mavlink_message_t *msg); + void handle_message_open_drone_id_operator_id(mavlink_message_t *msg); void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_ping(mavlink_message_t *msg); void handle_message_play_tune(mavlink_message_t *msg); @@ -308,6 +310,7 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; uORB::Publication _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)}; + uORB::Publication _open_drone_id_operator_id_pub{ORB_ID(open_drone_id_operator_id)}; uORB::Publication _generator_status_pub{ORB_ID(generator_status)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; From 6bb05af7ba8488f7693e39127fe010eeaf0deb40 Mon Sep 17 00:00:00 2001 From: jnomikos Date: Mon, 3 Jul 2023 01:47:48 -0500 Subject: [PATCH 2/8] System id added to mavlink receiver --- msg/CMakeLists.txt | 1 + msg/OpenDroneIdSystem.msg | 15 ++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 30 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 3 +++ 4 files changed, 49 insertions(+) create mode 100644 msg/OpenDroneIdSystem.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3b251b7b3e21..92f6e81f98fa 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -139,6 +139,7 @@ set(msg_files OnboardComputerStatus.msg OpenDroneIdArmStatus.msg OpenDroneIdOperatorId.msg + OpenDroneIdSystem.msg OrbitStatus.msg OrbTest.msg OrbTestLarge.msg diff --git a/msg/OpenDroneIdSystem.msg b/msg/OpenDroneIdSystem.msg new file mode 100644 index 000000000000..ed065d4761e1 --- /dev/null +++ b/msg/OpenDroneIdSystem.msg @@ -0,0 +1,15 @@ +uint64 timestamp +uint8 target_system +uint8 target_component +uint8[20] id_or_mac +uint8 operator_location_type +uint8 classification_type +uint32 operator_latitude +uint32 operator_longitude +uint16 area_count +uint16 area_radius +float32 area_ceiling +float32 area_floor +uint8 category_eu +uint8 class_eu +float32 operator_altitude_geo diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index eb73e41b8a0b..675dc602ba39 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -292,6 +292,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_open_drone_id_operator_id(msg); break; + case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM: + handle_message_open_drone_id_system(msg); + break; + #if !defined(CONSTRAINED_FLASH) case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: @@ -3113,6 +3117,32 @@ void MavlinkReceiver::handle_message_open_drone_id_operator_id( _open_drone_id_operator_id_pub.publish(odid_operator_id); } +void MavlinkReceiver::handle_message_open_drone_id_system( + mavlink_message_t *msg) +{ + mavlink_open_drone_id_system_t odid_module; + mavlink_msg_open_drone_id_system_decode(msg, &odid_module); + + open_drone_id_system_s odid_system{}; + memset(&odid_system, 0, sizeof(odid_system)); + + odid_system.timestamp = hrt_absolute_time(); + odid_system.target_system = odid_module.target_system; + odid_system.target_component = odid_module.target_component; + memcpy(odid_system.id_or_mac, odid_module.id_or_mac, sizeof(odid_system.id_or_mac)); + odid_system.operator_location_type = odid_module.operator_location_type; + odid_system.classification_type = odid_module.classification_type; + odid_system.operator_latitude = odid_module.operator_latitude; + odid_system.operator_longitude = odid_module.operator_longitude; + odid_system.area_count = odid_module.area_count; + odid_system.area_radius = odid_module.area_radius; + odid_system.area_ceiling = odid_module.area_ceiling; + odid_system.area_floor = odid_module.area_floor; + odid_system.category_eu = odid_module.category_eu; + odid_system.class_eu = odid_module.class_eu; + odid_system.operator_altitude_geo = odid_module.operator_altitude_geo; +} + void MavlinkReceiver::run() { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a924bec93bb9..bdf0e9654cbb 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -89,6 +89,7 @@ #include #include #include +#include #include #include #include @@ -183,6 +184,7 @@ class MavlinkReceiver : public ModuleParams void handle_message_onboard_computer_status(mavlink_message_t *msg); void handle_message_open_drone_id_arm_status(mavlink_message_t *msg); void handle_message_open_drone_id_operator_id(mavlink_message_t *msg); + void handle_message_open_drone_id_system(mavlink_message_t *msg); void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_ping(mavlink_message_t *msg); void handle_message_play_tune(mavlink_message_t *msg); @@ -311,6 +313,7 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; uORB::Publication _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)}; uORB::Publication _open_drone_id_operator_id_pub{ORB_ID(open_drone_id_operator_id)}; + uORB::Publication _open_drone_id_system_pub{ORB_ID(open_drone_id_system)}; uORB::Publication _generator_status_pub{ORB_ID(generator_status)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; From bb184fb15ed81ba16e51f36f954ddef028359a0c Mon Sep 17 00:00:00 2001 From: jnomikos Date: Mon, 3 Jul 2023 02:00:51 -0500 Subject: [PATCH 3/8] Self id added to mavlink receiver --- msg/CMakeLists.txt | 1 + msg/OpenDroneIdSelfId.msg | 6 ++++++ src/modules/mavlink/mavlink_receiver.cpp | 19 +++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 3 +++ 4 files changed, 29 insertions(+) create mode 100644 msg/OpenDroneIdSelfId.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 92f6e81f98fa..015436541498 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -139,6 +139,7 @@ set(msg_files OnboardComputerStatus.msg OpenDroneIdArmStatus.msg OpenDroneIdOperatorId.msg + OpenDroneIdSelfId.msg OpenDroneIdSystem.msg OrbitStatus.msg OrbTest.msg diff --git a/msg/OpenDroneIdSelfId.msg b/msg/OpenDroneIdSelfId.msg new file mode 100644 index 000000000000..a176f94a87dc --- /dev/null +++ b/msg/OpenDroneIdSelfId.msg @@ -0,0 +1,6 @@ +uint64 timestamp +uint8 target_system +uint8 target_component +uint8[20] id_or_mac +uint8 description_type +char[23] description diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 675dc602ba39..afdd7b9ed9d8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3117,6 +3117,25 @@ void MavlinkReceiver::handle_message_open_drone_id_operator_id( _open_drone_id_operator_id_pub.publish(odid_operator_id); } +void MavlinkReceiver::handle_message_open_drone_id_self_id( + mavlink_message_t *msg) +{ + mavlink_open_drone_id_self_id_t odid_module; + mavlink_msg_open_drone_id_self_id_decode(msg, &odid_module); + + open_drone_id_self_id_s odid_self_id{}; + memset(&odid_self_id, 0, sizeof(odid_self_id)); + + odid_self_id.timestamp = hrt_absolute_time(); + odid_self_id.target_system = odid_module.target_system; + odid_self_id.target_component = odid_module.target_component; + memcpy(odid_self_id.id_or_mac, odid_module.id_or_mac, sizeof(odid_self_id.id_or_mac)); + odid_self_id.description_type = odid_module.description_type; + memcpy(odid_self_id.description, odid_module.description, sizeof(odid_self_id.description)); + + _open_drone_id_self_id_pub.publish(odid_self_id); +} + void MavlinkReceiver::handle_message_open_drone_id_system( mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index bdf0e9654cbb..6c380e4ad5d2 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -89,6 +89,7 @@ #include #include #include +#include #include #include #include @@ -184,6 +185,7 @@ class MavlinkReceiver : public ModuleParams void handle_message_onboard_computer_status(mavlink_message_t *msg); void handle_message_open_drone_id_arm_status(mavlink_message_t *msg); void handle_message_open_drone_id_operator_id(mavlink_message_t *msg); + void handle_message_open_drone_id_self_id(mavlink_message_t *msg); void handle_message_open_drone_id_system(mavlink_message_t *msg); void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_ping(mavlink_message_t *msg); @@ -313,6 +315,7 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; uORB::Publication _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)}; uORB::Publication _open_drone_id_operator_id_pub{ORB_ID(open_drone_id_operator_id)}; + uORB::Publication _open_drone_id_self_id_pub{ORB_ID(open_drone_id_self_id)}; uORB::Publication _open_drone_id_system_pub{ORB_ID(open_drone_id_system)}; uORB::Publication _generator_status_pub{ORB_ID(generator_status)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; From ae4551b18ef9894925635dc1f729bb69461d5d91 Mon Sep 17 00:00:00 2001 From: jnomikos Date: Thu, 6 Jul 2023 15:15:40 -0500 Subject: [PATCH 4/8] Self_ID, calls handler when mavlink message recieved --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index afdd7b9ed9d8..d540010ecd36 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -292,6 +292,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_open_drone_id_operator_id(msg); break; + case MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID: + handle_message_open_drone_id_self_id(msg); + break; + case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM: handle_message_open_drone_id_system(msg); break; From c6996d2c9fb688d79b241448b1137b346dbc0722 Mon Sep 17 00:00:00 2001 From: jnomikos Date: Thu, 6 Jul 2023 15:33:54 -0500 Subject: [PATCH 5/8] Operator and self id now subscribed to uORB messages --- .../mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp | 11 ++++++++--- src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp | 11 ++++++++--- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp index fd8ec6e071b7..1ee4ffe1ea5a 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp @@ -35,6 +35,7 @@ #define OPEN_DRONE_ID_OPERATOR_ID_HPP #include +#include class MavlinkStreamOpenDroneIdOperatorId : public MavlinkStream { @@ -68,17 +69,21 @@ class MavlinkStreamOpenDroneIdOperatorId : public MavlinkStream : MavlinkStream(mavlink) {} uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _open_drone_id_operator_id_sub{ORB_ID(open_drone_id_operator_id)}; bool send() override { + open_drone_id_operator_id_s operator_id{}; + _open_drone_id_operator_id_sub.copy(&operator_id); + mavlink_open_drone_id_operator_id_t msg{}; - msg.target_component = 0; // 0 for broadcast msg.target_system = 0; // 0 for broadcast + msg.target_component = 0; // 0 for broadcast // msg.id_or_mac // Only used for drone ID data received from other UAs. - msg.operator_id_type = 0; + msg.operator_id_type = operator_id.operator_id_type; for (uint8_t i = 0; i < 20; ++i) { - msg.operator_id[i] = '\0'; + msg.operator_id[i] = operator_id.operator_id[i]; } mavlink_msg_open_drone_id_operator_id_send_struct(_mavlink->get_channel(), diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp index 6c0af7f2269c..bb4ad1a280e6 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp @@ -35,6 +35,7 @@ #define OPEN_DRONE_ID_SELF_ID_HPP #include +#include class MavlinkStreamOpenDroneIdSelfId : public MavlinkStream { @@ -67,17 +68,21 @@ class MavlinkStreamOpenDroneIdSelfId : public MavlinkStream : MavlinkStream(mavlink) {} uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _open_drone_id_self_id_sub{ORB_ID(open_drone_id_self_id)}; bool send() override { + open_drone_id_self_id_s self_id; + _open_drone_id_self_id_sub.copy(&self_id); + mavlink_open_drone_id_self_id_t msg{}; - msg.target_component = 0; // 0 for broadcast msg.target_system = 0; // 0 for broadcast + msg.target_component = 0; // 0 for broadcast // msg.id_or_mac // Only used for drone ID data received from other UAs. - msg.description_type = 0; + msg.description_type = self_id.description_type; for (uint8_t i = 0; i < 23; ++i) { - msg.description[i] = '\0'; + msg.description[i] = self_id.description[i]; } mavlink_msg_open_drone_id_self_id_send_struct(_mavlink->get_channel(), From 68309e020cc37c4d7471891347c86dc36b569aab Mon Sep 17 00:00:00 2001 From: jnomikos Date: Thu, 6 Jul 2023 16:05:45 -0500 Subject: [PATCH 6/8] changed .copy to .update and made it apart of an if check --- .../streams/OPEN_DRONE_ID_OPERATOR_ID.hpp | 28 +++++++++++-------- .../mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp | 28 +++++++++++-------- 2 files changed, 32 insertions(+), 24 deletions(-) diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp index 1ee4ffe1ea5a..e205e1f32777 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp @@ -74,22 +74,26 @@ class MavlinkStreamOpenDroneIdOperatorId : public MavlinkStream bool send() override { open_drone_id_operator_id_s operator_id{}; - _open_drone_id_operator_id_sub.copy(&operator_id); - mavlink_open_drone_id_operator_id_t msg{}; - msg.target_system = 0; // 0 for broadcast - msg.target_component = 0; // 0 for broadcast - // msg.id_or_mac // Only used for drone ID data received from other UAs. - msg.operator_id_type = operator_id.operator_id_type; + if(_open_drone_id_operator_id_sub.update(&operator_id)) { - for (uint8_t i = 0; i < 20; ++i) { - msg.operator_id[i] = operator_id.operator_id[i]; - } + mavlink_open_drone_id_operator_id_t msg{}; + msg.target_system = 0; // 0 for broadcast + msg.target_component = 0; // 0 for broadcast + // msg.id_or_mac // Only used for drone ID data received from other UAs. + msg.operator_id_type = operator_id.operator_id_type; + + for (uint8_t i = 0; i < 20; ++i) { + msg.operator_id[i] = operator_id.operator_id[i]; + } - mavlink_msg_open_drone_id_operator_id_send_struct(_mavlink->get_channel(), - &msg); + mavlink_msg_open_drone_id_operator_id_send_struct(_mavlink->get_channel(), + &msg); + + return true; + } - return true; + return false; } }; diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp index bb4ad1a280e6..fe19136394d0 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp @@ -73,22 +73,26 @@ class MavlinkStreamOpenDroneIdSelfId : public MavlinkStream bool send() override { open_drone_id_self_id_s self_id; - _open_drone_id_self_id_sub.copy(&self_id); - mavlink_open_drone_id_self_id_t msg{}; - msg.target_system = 0; // 0 for broadcast - msg.target_component = 0; // 0 for broadcast - // msg.id_or_mac // Only used for drone ID data received from other UAs. - msg.description_type = self_id.description_type; + if(_open_drone_id_self_id_sub.update(&self_id)) { - for (uint8_t i = 0; i < 23; ++i) { - msg.description[i] = self_id.description[i]; - } + mavlink_open_drone_id_self_id_t msg{}; + msg.target_system = 0; // 0 for broadcast + msg.target_component = 0; // 0 for broadcast + // msg.id_or_mac // Only used for drone ID data received from other UAs. + msg.description_type = self_id.description_type; + + for (uint8_t i = 0; i < 23; ++i) { + msg.description[i] = self_id.description[i]; + } - mavlink_msg_open_drone_id_self_id_send_struct(_mavlink->get_channel(), - &msg); + mavlink_msg_open_drone_id_self_id_send_struct(_mavlink->get_channel(), + &msg); + + return true; + } - return true; + return false; } }; From c75a42bb9f55a49504a9eca992568d3edf79ecf7 Mon Sep 17 00:00:00 2001 From: jnomikos Date: Thu, 6 Jul 2023 17:30:46 -0500 Subject: [PATCH 7/8] OPEN_DRONE_ID_SYSTEM now gets info from uORB if available --- .../mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp | 36 +++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp index 8f73ec39aa95..5600f1dd38ae 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp @@ -36,6 +36,7 @@ #include #include +#include class MavlinkStreamOpenDroneIdSystem : public MavlinkStream { @@ -74,20 +75,21 @@ class MavlinkStreamOpenDroneIdSystem : public MavlinkStream uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _open_drone_id_system_sub{ORB_ID(open_drone_id_system)}; bool send() override { sensor_gps_s vehicle_gps_position; home_position_s home_position; - + open_drone_id_system_s odid_system; if (_vehicle_gps_position_sub.update(&vehicle_gps_position) && _home_position_sub.copy(&home_position)) { if (vehicle_gps_position.fix_type >= 3 && home_position.valid_alt && home_position.valid_hpos) { mavlink_open_drone_id_system_t msg{}; - msg.target_component = 0; // 0 for broadcast msg.target_system = 0; // 0 for broadcast + msg.target_component = 0; // 0 for broadcast // msg.id_or_mac // Only used for drone ID data received from other UAs. msg.operator_location_type = MAV_ODID_OPERATOR_LOCATION_TYPE_TAKEOFF; msg.classification_type = MAV_ODID_CLASSIFICATION_TYPE_UNDECLARED; @@ -114,6 +116,36 @@ class MavlinkStreamOpenDroneIdSystem : public MavlinkStream } } + + if (_vehicle_gps_position_sub.update(&vehicle_gps_position) && + _home_position_sub.copy(&home_position) && _open_drone_id_system_sub.update(&odid_system)) { + if (vehicle_gps_position.fix_type >= 3 && home_position.valid_alt && + home_position.valid_hpos) { + + mavlink_open_drone_id_system_t msg{}; + + msg.target_system = 0; // 0 for broadcast + msg.target_component = 0; // 0 for broadcast + // msg.id_or_mac // Only used for drone ID data received from other UAs. + msg.operator_location_type = odid_system.operator_location_type; + msg.classification_type = odid_system.classification_type; + msg.operator_latitude = odid_system.operator_latitude; + msg.operator_longitude = odid_system.operator_longitude; + msg.area_count = odid_system.area_count; + msg.area_radius = odid_system.area_radius; + msg.area_ceiling = odid_system.area_ceiling; + msg.area_floor = odid_system.area_floor; + msg.category_eu = odid_system.category_eu; + msg.class_eu = odid_system.class_eu; + msg.operator_altitude_geo = odid_system.operator_altitude_geo; + + msg.timestamp = odid_system.timestamp; + + mavlink_msg_open_drone_id_system_send_struct(_mavlink->get_channel(), + &msg); + return true; + } + } return false; } }; From 17f706b8b0d3724020e54c8abe88bb67fea29196 Mon Sep 17 00:00:00 2001 From: jnomikos Date: Fri, 7 Jul 2023 10:56:08 -0500 Subject: [PATCH 8/8] operator_latitude, operator_longitude, should be int32 --- msg/OpenDroneIdSystem.msg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/msg/OpenDroneIdSystem.msg b/msg/OpenDroneIdSystem.msg index ed065d4761e1..bcc5a8c42036 100644 --- a/msg/OpenDroneIdSystem.msg +++ b/msg/OpenDroneIdSystem.msg @@ -4,8 +4,8 @@ uint8 target_component uint8[20] id_or_mac uint8 operator_location_type uint8 classification_type -uint32 operator_latitude -uint32 operator_longitude +int32 operator_latitude +int32 operator_longitude uint16 area_count uint16 area_radius float32 area_ceiling