Merge pull request #1 from jnomikos/remote_id_arm

OPEN_DRONE_ID_SYSTEM, OPEN_DRONE_ID_SELF_ID, and OPEN_DRONE_ID_OPERATOR_ID values getting cached within PX4

Author: dirksavage88

msg/CMakeLists.txt

@@ -138,6 +138,9 @@ set(msg_files
 	OffboardControlMode.msg
 	OnboardComputerStatus.msg
         OpenDroneIdArmStatus.msg
+	OpenDroneIdOperatorId.msg
+	OpenDroneIdSelfId.msg
+	OpenDroneIdSystem.msg
 	OrbitStatus.msg
 	OrbTest.msg
 	OrbTestLarge.msg

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

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

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
+int32 operator_latitude
+int32 operator_longitude
+uint16 area_count
+uint16 area_radius
+float32 area_ceiling
+float32 area_floor
+uint8 category_eu
+uint8 class_eu
+float32 operator_altitude_geo

src/modules/mavlink/mavlink_receiver.cpp

@@ -288,6 +288,18 @@ 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;
+
+	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;
+
 #if !defined(CONSTRAINED_FLASH)
 
 	case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
@@ -3088,6 +3100,72 @@ 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::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)
+{
+	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()
 {

src/modules/mavlink/mavlink_receiver.h

@@ -88,6 +88,9 @@
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/onboard_computer_status.h>
 #include <uORB/topics/open_drone_id_arm_status.h>
+#include <uORB/topics/open_drone_id_operator_id.h>
+#include <uORB/topics/open_drone_id_self_id.h>
+#include <uORB/topics/open_drone_id_system.h>
 #include <uORB/topics/ping.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/radio_status.h>
@@ -181,6 +184,9 @@ 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_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);
 	void handle_message_play_tune(mavlink_message_t *msg);
@@ -308,6 +314,9 @@ class MavlinkReceiver : public ModuleParams
 	uORB::Publication<offboard_control_mode_s>		_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
 	uORB::Publication<onboard_computer_status_s>		_onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
 	uORB::Publication<open_drone_id_arm_status_s> _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)};
+	uORB::Publication<open_drone_id_operator_id_s> _open_drone_id_operator_id_pub{ORB_ID(open_drone_id_operator_id)};
+	uORB::Publication<open_drone_id_self_id_s> _open_drone_id_self_id_pub{ORB_ID(open_drone_id_self_id)};
+	uORB::Publication<open_drone_id_system_s> _open_drone_id_system_pub{ORB_ID(open_drone_id_system)};
 	uORB::Publication<generator_status_s>			_generator_status_pub{ORB_ID(generator_status)};
 	uORB::Publication<vehicle_attitude_s>			_attitude_pub{ORB_ID(vehicle_attitude)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};

src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp

@@ -35,6 +35,7 @@
 #define OPEN_DRONE_ID_OPERATOR_ID_HPP
 
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/open_drone_id_operator_id.h>
 
 class MavlinkStreamOpenDroneIdOperatorId : public MavlinkStream
 {
@@ -68,23 +69,31 @@ 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
 	{
-		mavlink_open_drone_id_operator_id_t msg{};
-		msg.target_component = 0; // 0 for broadcast
-		msg.target_system = 0;    // 0 for broadcast
-		// msg.id_or_mac // Only used for drone ID data received from other UAs.
-		msg.operator_id_type = 0;
-
-		for (uint8_t i = 0; i < 20; ++i) {
-			msg.operator_id[i] = '\0';
-		}
+		open_drone_id_operator_id_s operator_id{};
+
+		if(_open_drone_id_operator_id_sub.update(&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;
 
-		mavlink_msg_open_drone_id_operator_id_send_struct(_mavlink->get_channel(),
-				&msg);
+			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);
+
+			return true;
+		}
 
-		return true;
+		return false;
 	}
 };
 

src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp

@@ -35,6 +35,7 @@
 #define OPEN_DRONE_ID_SELF_ID_HPP
 
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/open_drone_id_self_id.h>
 
 class MavlinkStreamOpenDroneIdSelfId : public MavlinkStream
 {
@@ -67,23 +68,31 @@ 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
 	{
-		mavlink_open_drone_id_self_id_t msg{};
-		msg.target_component = 0; // 0 for broadcast
-		msg.target_system = 0;    // 0 for broadcast
-		// msg.id_or_mac // Only used for drone ID data received from other UAs.
-		msg.description_type = 0;
-
-		for (uint8_t i = 0; i < 23; ++i) {
-			msg.description[i] = '\0';
-		}
+		open_drone_id_self_id_s self_id;
+
+		if(_open_drone_id_self_id_sub.update(&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;
 
-		mavlink_msg_open_drone_id_self_id_send_struct(_mavlink->get_channel(),
-				&msg);
+			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);
+
+			return true;
+		}
 
-		return true;
+		return false;
 	}
 };
 

src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp

@@ -36,6 +36,7 @@
 
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/open_drone_id_system.h>
 
 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;
 	}
 };