From 1304562861c44df5fe71ca61d741ca9cafd46546 Mon Sep 17 00:00:00 2001
From: Jari Nippula <jarix@ssrc.tii.ae>
Date: Tue, 16 Jan 2024 11:27:32 +0200
Subject: [PATCH] M-O-I commander to request critical action

---
 src/modules/commander/Commander.cpp | 56 +++++++++++++++++++++++++++++
 src/modules/commander/Commander.hpp |  4 +++
 2 files changed, 60 insertions(+)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 8ca650156535..c58bf7ec325e 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -533,6 +533,55 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
 
 transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks)
 {
+	bool moi_critical_allowed = false;
+	bool moi_response_received = false;
+	hrt_abstime now = hrt_absolute_time();
+
+	PX4_INFO("M-O-I check..");
+	vehicle_command_s moi_critical_req{};
+	moi_critical_req.timestamp = now;
+	moi_critical_req.command = vehicle_command_s::VEHICLE_CMD_CUSTOM_1;
+	_crit_act_req_pub.publish(moi_critical_req);
+
+	while (!moi_response_received && hrt_elapsed_time(&now) < 1_s) {
+
+		vehicle_command_ack_s moi_critical_resp{};
+
+		if (_crit_act_resp_sub.update(&moi_critical_resp)) {
+
+			switch (moi_critical_resp.command) {
+			case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: {
+					moi_response_received = true;
+
+					if (moi_critical_resp.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED)	{
+						moi_critical_allowed = true;
+						PX4_INFO("M-O-I RESP - ALLOWED");
+
+					} else if (moi_critical_resp.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
+						moi_critical_allowed = false;
+						PX4_INFO("M-O-I RESP - DENIED");
+
+					} else {
+						PX4_WARN("M-O-I RESP - UKNOWN RESPONSE! %d", moi_critical_resp.result);
+					}
+				}
+				break;
+
+			default:
+				break;
+			}
+		}
+
+		if (!moi_response_received) {
+			px4_usleep(1000);
+		}
+	}
+
+	if (!moi_critical_allowed) {
+		PX4_WARN("ARMING DENIED: MOI critical action not allowed!");
+		return TRANSITION_DENIED;
+	}
+
 	// allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming
 	if (calling_reason == arm_disarm_reason_t::rc_switch
 	    && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) {
@@ -594,6 +643,8 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 
 transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool forced)
 {
+	vehicle_command_s moi_critical_req{};
+
 	if (!forced) {
 		const bool landed = (_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed
 				     || is_ground_vehicle(_vehicle_status));
@@ -635,6 +686,11 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 		tune_negative(true);
 	}
 
+	PX4_INFO("M-O-I Critical action release");
+	moi_critical_req.timestamp = hrt_absolute_time();
+	moi_critical_req.command = vehicle_command_s::VEHICLE_CMD_CUSTOM_0;
+	_crit_act_req_pub.publish(moi_critical_req);
+
 	return arming_res;
 }
 
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index a29ac4b65411..52092f506c8d 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -299,6 +299,10 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	uORB::Publication<vehicle_control_mode_s>		_vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
 	uORB::Publication<vehicle_status_s>			_vehicle_status_pub{ORB_ID(vehicle_status)};
 
+	// M-O-I critical action handling
+	uORB::Publication<vehicle_command_s> 			_crit_act_req_pub{ORB_ID(moi_critical_req)};
+	uORB::Subscription					_crit_act_resp_sub{ORB_ID(moi_critical_resp)};
+
 	orb_advert_t _mavlink_log_pub{nullptr};
 
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};