Open Drone ID basic serial support via mavlink forwarding and streams…

…. Removed all streams that QGC sends currently.

Signed-off-by: dirksavage88 <[email protected]>

msg/CMakeLists.txt

@@ -139,6 +139,11 @@ set(msg_files
 	ObstacleDistance.msg
 	OffboardControlMode.msg
 	OnboardComputerStatus.msg
+  OpenDroneIdArmStatus.msg
+  OpenDroneIdBasicId.msg
+	OpenDroneIdOperatorId.msg
+	OpenDroneIdSelfId.msg
+	OpenDroneIdSystem.msg
 	OrbitStatus.msg
 	OrbTest.msg
 	OrbTestLarge.msg

msg/OpenDroneIdArmStatus.msg

@@ -0,0 +1,3 @@
+uint64 timestamp
+uint8 status
+char[50] error

msg/OpenDroneIdBasicId.msg

@@ -0,0 +1,7 @@
+uint64 timestamp
+uint8 target_system
+uint8 target_component
+uint8[20] id_or_mac
+uint8 id_type
+uint8 ua_type
+uint8[20] uas_id

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/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -176,4 +176,3 @@ class HealthAndArmingChecks : public ModuleParams
 		&_vtol_checks,
 	};
 };
-

src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.cpp

@@ -33,6 +33,7 @@
 
 #include "openDroneIDCheck.hpp"
 
+#define ODID_ARM_FAIL 1
 
 void OpenDroneIDChecks::checkAndReport(const Context &context, Report &reporter)
 {
@@ -41,46 +42,56 @@ void OpenDroneIDChecks::checkAndReport(const Context &context, Report &reporter)
 		return;
 	}
 
-	NavModes affected_modes{NavModes::None};
+	// ODID module arm status topic
+	open_drone_id_arm_status_s odid_module_arm_status;
 
-	if (_param_com_arm_odid.get() == 2) {
-		// disallow arming without the Open Drone ID system
-		affected_modes = NavModes::All;
-	}
+	NavModes affected_modes{NavModes::None};
 
-	if (!context.status().open_drone_id_system_present) {
-		/* EVENT
-		 * @description
-		 * Open Drone ID system failed to report. Make sure it is setup and installed properly.
-		 *
-		 * <profile name="dev">
-		 * This check can be configured via <param>COM_ARM_ODID</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure(affected_modes, health_component_t::open_drone_id,
-					    events::ID("check_open_drone_id_missing"),
-					    events::Log::Error, "Open Drone ID system missing");
+	if (_open_drone_id_arm_status_sub.copy(&odid_module_arm_status)) {
 
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Open Drone ID system missing");
+		if (_param_com_arm_odid.get() == 2) {
+			// disallow arming without the Open Drone ID system
+			affected_modes = NavModes::All;
 		}
 
-	} else if (!context.status().open_drone_id_system_healthy) {
-		/* EVENT
-		 * @description
-		 * Open Drone ID system reported being unhealthy.
-		 *
-		 * <profile name="dev">
-		 * This check can be configured via <param>COM_ARM_ODID</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure(affected_modes, health_component_t::open_drone_id,
-					    events::ID("check_open_drone_id_unhealthy"),
-					    events::Log::Error, "Open Drone ID system not ready");
+		if (!context.status().open_drone_id_system_present) {
+			/* EVENT
+			* @description
+			* Open Drone ID system failed to report. Make sure it is setup and installed properly.
+			*
+			* <profile name="dev">
+			* This check can be configured via <param>COM_ARM_ODID</param> parameter.
+			* </profile>
+			*/
+			reporter.armingCheckFailure(affected_modes, health_component_t::open_drone_id,
+						    events::ID("check_open_drone_id_missing"),
+						    events::Log::Error, "Open Drone ID system missing");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Open Drone ID system missing");
+			}
+
+			// Check ODID arm status
+
+		} else if (odid_module_arm_status.status == ODID_ARM_FAIL) {
+			/* EVENT
+			* @description
+			* Open Drone ID system reported being unhealthy.
+			*
+			* <profile name="dev">
+			* This check can be ignored or set to warning via <param>COM_ARM_ODID</param> parameter.
+			* </profile>
+			*/
+			reporter.armingCheckFailure(affected_modes, health_component_t::open_drone_id,
+						    events::ID("check_open_drone_id_unhealthy"),
+						    events::Log::Error, "Open Drone ID arm status error");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Open Drone ID arm status error");
+			}
 
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Open Drone ID system not ready");
 		}
 
 	}
+
 }

src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.hpp

@@ -34,6 +34,8 @@
 #pragma once
 
 #include "../Common.hpp"
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/open_drone_id_arm_status.h>
 
 class OpenDroneIDChecks : public HealthAndArmingCheckBase
 {
@@ -47,4 +49,5 @@ class OpenDroneIDChecks : public HealthAndArmingCheckBase
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamInt<px4::params::COM_ARM_ODID>) _param_com_arm_odid
 				       )
+	uORB::Subscription _open_drone_id_arm_status_sub{ORB_ID(open_drone_id_arm_status)};
 };

src/modules/mavlink/mavlink_main.cpp

@@ -1509,13 +1509,13 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 		configure_stream_local("GPS_RAW_INT", 1.0f);
 		configure_stream_local("GPS_STATUS", 1.0f);
-		configure_stream_local("HOME_POSITION", 0.5f);
+		configure_stream_local("HOME_POSITION", 1.5f);
 		configure_stream_local("HYGROMETER_SENSOR", 0.1f);
 		configure_stream_local("LOCAL_POSITION_NED", 1.0f);
 		configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.0f);
 		configure_stream_local("OBSTACLE_DISTANCE", 1.0f);
+		configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f);
 		configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f);
-		configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 2.0f);
 		configure_stream_local("PING", 0.1f);
 		configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.0f);
@@ -1573,11 +1573,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 		configure_stream_local("GPS_RAW_INT", unlimited_rate);
 		configure_stream_local("GPS_STATUS", 1.0f);
-		configure_stream_local("HOME_POSITION", 0.5f);
+		configure_stream_local("HOME_POSITION", 1.5f);
 		configure_stream_local("HYGROMETER_SENSOR", 1.0f);
 		configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
+		configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f);
 		configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f);
-		configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f);
 		configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
 		configure_stream_local("PING", 1.0f);
@@ -1723,13 +1723,13 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GPS_RAW_INT", unlimited_rate);
 		configure_stream_local("GPS_STATUS", 1.0f);
 		configure_stream_local("HIGHRES_IMU", 50.0f);
-		configure_stream_local("HOME_POSITION", 0.5f);
+		configure_stream_local("HOME_POSITION", 1.5f);
 		configure_stream_local("HYGROMETER_SENSOR", 1.0f);
 		configure_stream_local("MAG_CAL_REPORT", 1.0f);
 		configure_stream_local("MANUAL_CONTROL", 5.0f);
 		configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
+		configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f);
 		configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f);
-		configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f);
 		configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
 		configure_stream_local("PING", 1.0f);
@@ -1809,10 +1809,10 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 		configure_stream_local("GPS2_RAW", unlimited_rate);
 		configure_stream_local("GPS_RAW_INT", unlimited_rate);
-		configure_stream_local("HOME_POSITION", 0.5f);
+		configure_stream_local("HOME_POSITION", 1.5f);
 		configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f);
+		configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f);
 		configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f);
-		configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f);
 		configure_stream_local("OPTICAL_FLOW_RAD", 1.0f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
 		configure_stream_local("PING", 0.1f);

src/modules/mavlink/mavlink_messages.cpp

@@ -94,9 +94,8 @@
 #include "streams/MOUNT_ORIENTATION.hpp"
 #include "streams/NAV_CONTROLLER_OUTPUT.hpp"
 #include "streams/OBSTACLE_DISTANCE.hpp"
-#include "streams/OPEN_DRONE_ID_BASIC_ID.hpp"
+#include "streams/OPEN_DRONE_ID_ARM_STATUS.hpp"
 #include "streams/OPEN_DRONE_ID_LOCATION.hpp"
-#include "streams/OPEN_DRONE_ID_SYSTEM.hpp"
 #include "streams/OPTICAL_FLOW_RAD.hpp"
 #include "streams/ORBIT_EXECUTION_STATUS.hpp"
 #include "streams/PING.hpp"
@@ -429,12 +428,21 @@ static const StreamListItem streams_list[] = {
 #if defined(OBSTACLE_DISTANCE_HPP)
 	create_stream_list_item<MavlinkStreamObstacleDistance>(),
 #endif // OBSTACLE_DISTANCE_HPP
+#if defined(OPEN_DRONE_ID_ARM_STATUS_HPP)
+	create_stream_list_item<MavlinkStreamOpenDroneIdArmStatus>(),
+#endif // OPEN_DRONE_ID_ARM_STATUS_HPP
 #if defined(OPEN_DRONE_ID_BASIC_ID_HPP)
 	create_stream_list_item<MavlinkStreamOpenDroneIdBasicId>(),
 #endif // OPEN_DRONE_ID_BASIC_ID_HPP
 #if defined(OPEN_DRONE_ID_LOCATION_HPP)
 	create_stream_list_item<MavlinkStreamOpenDroneIdLocation>(),
 #endif // OPEN_DRONE_ID_LOCATION_HPP
+#if defined(OPEN_DRONE_ID_OPERATOR_ID_HPP)
+	create_stream_list_item<MavlinkStreamOpenDroneIdOperatorId>(),
+#endif // OPEN_DRONE_ID_OPERATOR_ID_HPP
+#if defined(OPEN_DRONE_ID_SELF_ID_HPP)
+	create_stream_list_item<MavlinkStreamOpenDroneIdSelfId>(),
+#endif // OPEN_DRONE_ID_SELF_ID_HPP
 #if defined(OPEN_DRONE_ID_SYSTEM_HPP)
 	create_stream_list_item<MavlinkStreamOpenDroneIdSystem>(),
 #endif // OPEN_DRONE_ID_SYSTEM_HPP