uavcan: implement OpenDroneID System

src/drivers/uavcan/remoteid.cpp

@@ -43,7 +43,8 @@ UavcanRemoteIDController::UavcanRemoteIDController(uavcan::INode &node) :
 	_timer(node),
 	_node(node),
 	_uavcan_pub_remoteid_basicid(node),
-	_uavcan_pub_remoteid_location(node)
+	_uavcan_pub_remoteid_location(node),
+	_uavcan_pub_remoteid_system(node)
 {
 }
 
@@ -61,6 +62,7 @@ void UavcanRemoteIDController::periodic_update(const uavcan::TimerEvent &)
 
 	send_basic_id();
 	send_location();
+	send_system();
 }
 
 void UavcanRemoteIDController::send_basic_id()
@@ -218,3 +220,37 @@ void UavcanRemoteIDController::send_location()
 		_uavcan_pub_remoteid_location.broadcast(msg);
 	}
 }
+
+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);
+		}
+	}
+}

src/drivers/uavcan/remoteid.hpp

@@ -44,6 +44,7 @@
 #include <uavcan/uavcan.hpp>
 #include <dronecan/remoteid/BasicID.hpp>
 #include <dronecan/remoteid/Location.hpp>
+#include <dronecan/remoteid/System.hpp>
 
 #include <px4_platform_common/module_params.h>
 
@@ -66,6 +67,7 @@ class UavcanRemoteIDController : public ModuleParams
 
 	void send_basic_id();
 	void send_location();
+	void send_system();
 
 	uavcan::INode &_node;
 
@@ -78,4 +80,5 @@ class UavcanRemoteIDController : public ModuleParams
 
 	uavcan::Publisher<dronecan::remoteid::BasicID> _uavcan_pub_remoteid_basicid;
 	uavcan::Publisher<dronecan::remoteid::Location> _uavcan_pub_remoteid_location;
+	uavcan::Publisher<dronecan::remoteid::System> _uavcan_pub_remoteid_system;
 };