remoteid: implement System as sent from GCS

This will send the System message if it is already being sent by a ground
station. Otherwise, it will assemble the message itself using the
takeoff/home location.

msg/CMakeLists.txt

@@ -155,6 +155,7 @@ set(msg_files
 	OpenDroneIdArmStatus.msg
 	OpenDroneIdOperatorId.msg
 	OpenDroneIdSelfId.msg
+	OpenDroneIdSystem.msg
 	OrbitStatus.msg
 	OrbTest.msg
 	OrbTestLarge.msg

msg/OpenDroneIdSystem.msg

@@ -0,0 +1,13 @@
+uint64 timestamp
+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/drivers/uavcan/remoteid.cpp

@@ -236,34 +236,65 @@ void UavcanRemoteIDController::send_location()
 
 void UavcanRemoteIDController::send_system()
 {
-	sensor_gps_s vehicle_gps_position;
-	home_position_s home_position;
-
-	if (_vehicle_gps_position_sub.copy(&vehicle_gps_position) && _home_position_sub.copy(&home_position)) {
-		if (vehicle_gps_position.fix_type >= 3
-		    && home_position.valid_alt && home_position.valid_hpos) {
-
-			dronecan::remoteid::System msg {};
-
-			// 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;
-			msg.operator_latitude = home_position.lat * 1e7;
-			msg.operator_longitude = home_position.lon * 1e7;
-			msg.area_count = 1;
-			msg.area_radius = 0;
-			msg.area_ceiling = -1000;
-			msg.area_floor = -1000;
-			msg.category_eu = MAV_ODID_CATEGORY_EU_UNDECLARED;
-			msg.class_eu = MAV_ODID_CLASS_EU_UNDECLARED;
-			float wgs84_amsl_offset = vehicle_gps_position.altitude_ellipsoid_m - vehicle_gps_position.altitude_msl_m;
-			msg.operator_altitude_geo = home_position.alt + wgs84_amsl_offset;
-
-			// timestamp: 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019.
-			static uint64_t utc_offset_s = 1'546'300'800; // UTC seconds since 00:00:00 01/01/2019
-			msg.timestamp = vehicle_gps_position.time_utc_usec / 1e6 - utc_offset_s;
-
-			_uavcan_pub_remoteid_system.broadcast(msg);
+	open_drone_id_system_s system;
+
+	if (_open_drone_id_system.advertised() && _open_drone_id_system.copy(&system)) {
+
+		// Use what ground station sends us.
+
+		dronecan::remoteid::System msg {};
+		msg.timestamp = system.timestamp;
+
+		for (unsigned i = 0; i < sizeof(system.id_or_mac); ++i) {
+			msg.id_or_mac.push_back(system.id_or_mac[i]);
+		}
+
+		msg.operator_location_type = system.operator_location_type;
+		msg.classification_type = system.classification_type;
+		msg.operator_latitude = system.operator_latitude;
+		msg.operator_longitude = system.operator_longitude;
+		msg.area_count = system.area_count;
+		msg.area_radius = system.area_radius;
+		msg.area_ceiling = system.area_ceiling;
+		msg.area_floor = system.area_floor;
+		msg.category_eu = system.category_eu;
+		msg.class_eu = system.class_eu;
+		msg.operator_altitude_geo = system.operator_altitude_geo;
+
+		_uavcan_pub_remoteid_system.broadcast(msg);
+
+	} else {
+		// And otherwise, send our home/takeoff location.
+
+		sensor_gps_s vehicle_gps_position;
+		home_position_s home_position;
+
+		if (_vehicle_gps_position_sub.copy(&vehicle_gps_position) && _home_position_sub.copy(&home_position)) {
+			if (vehicle_gps_position.fix_type >= 3
+			    && home_position.valid_alt && home_position.valid_hpos) {
+
+				dronecan::remoteid::System msg {};
+
+				// 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;
+				msg.operator_latitude = home_position.lat * 1e7;
+				msg.operator_longitude = home_position.lon * 1e7;
+				msg.area_count = 1;
+				msg.area_radius = 0;
+				msg.area_ceiling = -1000;
+				msg.area_floor = -1000;
+				msg.category_eu = MAV_ODID_CATEGORY_EU_UNDECLARED;
+				msg.class_eu = MAV_ODID_CLASS_EU_UNDECLARED;
+				float wgs84_amsl_offset = vehicle_gps_position.altitude_ellipsoid_m - vehicle_gps_position.altitude_msl_m;
+				msg.operator_altitude_geo = home_position.alt + wgs84_amsl_offset;
+
+				// timestamp: 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019.
+				static uint64_t utc_offset_s = 1'546'300'800; // UTC seconds since 00:00:00 01/01/2019
+				msg.timestamp = vehicle_gps_position.time_utc_usec / 1e6 - utc_offset_s;
+
+				_uavcan_pub_remoteid_system.broadcast(msg);
+			}
 		}
 	}
 }

src/drivers/uavcan/remoteid.hpp

@@ -44,6 +44,7 @@
 #include <uORB/topics/open_drone_id_operator_id.h>
 #include <uORB/topics/open_drone_id_arm_status.h>
 #include <uORB/topics/open_drone_id_self_id.h>
+#include <uORB/topics/open_drone_id_system.h>
 
 #include <uavcan/uavcan.hpp>
 #include <dronecan/remoteid/BasicID.hpp>
@@ -90,6 +91,7 @@ class UavcanRemoteIDController : public ModuleParams
 	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
 	uORB::Subscription _open_drone_id_operator_id{ORB_ID(open_drone_id_operator_id)};
 	uORB::Subscription _open_drone_id_self_id{ORB_ID(open_drone_id_self_id)};
+	uORB::Subscription _open_drone_id_system{ORB_ID(open_drone_id_system)};
 	uORB::Publication<open_drone_id_arm_status_s> _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)};
 
 	uavcan::Publisher<dronecan::remoteid::BasicID> _uavcan_pub_remoteid_basicid;

src/modules/mavlink/mavlink_receiver.cpp

@@ -288,6 +288,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		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:
@@ -3089,6 +3093,31 @@ void MavlinkReceiver::handle_message_open_drone_id_self_id(mavlink_message_t *ms
 	_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();
+	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;
+
+	_open_drone_id_system_pub.publish(odid_system);
+}
 void
 MavlinkReceiver::run()
 {

src/modules/mavlink/mavlink_receiver.h

@@ -89,6 +89,7 @@
 #include <uORB/topics/onboard_computer_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>
@@ -318,6 +319,7 @@ class MavlinkReceiver : public ModuleParams
 	uORB::Publication<velocity_limits_s>			_velocity_limits_pub{ORB_ID(velocity_limits)};
 	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)};