ODID prototype

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

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -37,37 +37,37 @@
 
 #include <px4_platform_common/module_params.h>
 #include <uORB/Publication.hpp>
-#include <uORB/topics/failsafe_flags.h>
 #include <uORB/topics/health_report.h>
+#include <uORB/topics/failsafe_flags.h>
 
 #include "checks/accelerometerCheck.hpp"
 #include "checks/airspeedCheck.hpp"
 #include "checks/baroCheck.hpp"
-#include "checks/batteryCheck.hpp"
 #include "checks/cpuResourceCheck.hpp"
 #include "checks/distanceSensorChecks.hpp"
 #include "checks/escCheck.hpp"
 #include "checks/estimatorCheck.hpp"
 #include "checks/failureDetectorCheck.hpp"
-#include "checks/flightTimeCheck.hpp"
-#include "checks/geofenceCheck.hpp"
 #include "checks/gyroCheck.hpp"
-#include "checks/homePositionCheck.hpp"
 #include "checks/imuConsistencyCheck.hpp"
 #include "checks/magnetometerCheck.hpp"
 #include "checks/manualControlCheck.hpp"
-#include "checks/missionCheck.hpp"
+#include "checks/homePositionCheck.hpp"
 #include "checks/modeCheck.hpp"
-#include "checks/offboardCheck.hpp"
 #include "checks/parachuteCheck.hpp"
 #include "checks/powerCheck.hpp"
-#include "checks/rcAndDataLinkCheck.hpp"
 #include "checks/rcCalibrationCheck.hpp"
 #include "checks/remoteidCheck.hpp"
 #include "checks/sdcardCheck.hpp"
 #include "checks/systemCheck.hpp"
-#include "checks/vtolCheck.hpp"
+#include "checks/batteryCheck.hpp"
 #include "checks/windCheck.hpp"
+#include "checks/geofenceCheck.hpp"
+#include "checks/flightTimeCheck.hpp"
+#include "checks/missionCheck.hpp"
+#include "checks/rcAndDataLinkCheck.hpp"
+#include "checks/vtolCheck.hpp"
+#include "checks/offboardCheck.hpp"
 
 class HealthAndArmingChecks : public ModuleParams
 {
@@ -96,16 +96,12 @@ class HealthAndArmingChecks : public ModuleParams
 	/**
 	 * Query the mode requirements: check if a mode prevents arming
 	 */
-	bool modePreventsArming(uint8_t nav_state) const
-	{
-		return _reporter.modePreventsArming(nav_state);
-	}
+	bool modePreventsArming(uint8_t nav_state) const { return _reporter.modePreventsArming(nav_state); }
 
 	const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; }
 
 protected:
 	void updateParams() override;
-
 private:
 	failsafe_flags_s _failsafe_flags{};
 
@@ -114,8 +110,7 @@ class HealthAndArmingChecks : public ModuleParams
 	orb_advert_t _mavlink_log_pub{nullptr};
 
 	uORB::Publication<health_report_s> _health_report_pub{ORB_ID(health_report)};
-	uORB::Publication<failsafe_flags_s> _failsafe_flags_pub{
-		ORB_ID(failsafe_flags)};
+	uORB::Publication<failsafe_flags_s> _failsafe_flags_pub{ORB_ID(failsafe_flags)};
 
 	// all checks
 	AccelerometerChecks _accelerometer_checks;
@@ -163,14 +158,12 @@ class HealthAndArmingChecks : public ModuleParams
 		&_home_position_checks,
 		&_mission_checks,
 		&_offboard_checks, // must be after _estimator_checks
-		&_mode_checks, // must be after _estimator_checks, _home_position_checks,
-		// _mission_checks, _offboard_checks
+		&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks
 		&_parachute_checks,
 		&_power_checks,
 		&_rc_calibration_checks,
 		&_sd_card_checks,
-		&_system_checks, // must be after _estimator_checks &
-		// _home_position_checks
+		&_system_checks, // must be after _estimator_checks & _home_position_checks
 		&_battery_checks,
 		&_wind_checks,
 		&_geofence_checks, // must be after _home_position_checks

src/modules/mavlink/mavlink_main.cpp

@@ -1515,6 +1515,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		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);
@@ -1577,6 +1578,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("HOME_POSITION", 0.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);
@@ -1729,6 +1731,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		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);
@@ -1812,6 +1815,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GPS_RAW_INT", unlimited_rate);
 		configure_stream_local("HOME_POSITION", 0.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);

src/modules/mavlink/mavlink_messages.cpp

@@ -94,6 +94,7 @@
 #include "streams/MOUNT_ORIENTATION.hpp"
 #include "streams/NAV_CONTROLLER_OUTPUT.hpp"
 #include "streams/OBSTACLE_DISTANCE.hpp"
+#include "streams/OPEN_DRONE_ID_ARM_STATUS.hpp"
 #include "streams/OPEN_DRONE_ID_BASIC_ID.hpp"
 #include "streams/OPEN_DRONE_ID_LOCATION.hpp"
 #include "streams/OPEN_DRONE_ID_SYSTEM.hpp"
@@ -429,6 +430,9 @@ 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

src/modules/mavlink/mavlink_receiver.cpp

@@ -284,6 +284,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_statustext(msg);
 		break;
 
+	case MAVLINK_MSG_OPEN_DRONE_ID_ARM_STATUS:
+		handle_message_open_drone_id_arm_status(msg);
+		break;
+
 #if !defined(CONSTRAINED_FLASH)
 
 	case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
@@ -3069,7 +3073,21 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t
 
 	_gimbal_device_attitude_status_pub.publish(gimbal_attitude_status);
 }
+void MavlinkReceiver::handle_message_open_drone_id_arm_status(
+	mavlink_message_t *msg)
+{
+	mavlink_open_drone_id_arm_status_t odid_module;
+	mavlink_msg_open_drone_id_arm_status_decode(msg, &odid_module);
 
+	open_drone_id_arm_status_s odid_arm{};
+	memset(&odid_arm, 0, sizeof(odid_arm));
+
+	odid_arm.timestamp = hrt_absolute_time();
+	odid_arm.status = odid_module.status;
+	// odid_module.error;
+
+	_open_drone_id_arm_status_pub.publish(odid_arm);
+}
 void
 MavlinkReceiver::run()
 {

src/modules/mavlink/mavlink_receiver.h

@@ -87,6 +87,7 @@
 #include <uORB/topics/obstacle_distance.h>
 #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/ping.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/radio_status.h>
@@ -179,6 +180,7 @@ class MavlinkReceiver : public ModuleParams
 	void handle_message_obstacle_distance(mavlink_message_t *msg);
 	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_optical_flow_rad(mavlink_message_t *msg);
 	void handle_message_ping(mavlink_message_t *msg);
 	void handle_message_play_tune(mavlink_message_t *msg);
@@ -305,6 +307,7 @@ class MavlinkReceiver : public ModuleParams
 	uORB::Publication<obstacle_distance_s>			_obstacle_distance_pub{ORB_ID(obstacle_distance)};
 	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<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)};