[commander] handle mavlink command to set safety switch state

msg/VehicleCommand.msg

@@ -96,6 +96,7 @@ uint16 VEHICLE_CMD_LOGGING_STOP = 2511			# stop streaming ULog data
 uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600		# control starting/stopping transmitting data over the high latency link
 uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000    	# Command VTOL transition
 uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001	# Request arm authorization
+uint16 VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE = 5300
 uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001	# Prepare a payload deployment in the flight plan
 uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed payload deployment
 uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006            # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location.
@@ -165,6 +166,10 @@ int8 ARMING_ACTION_ARM = 1
 uint8 GRIPPER_ACTION_RELEASE = 0
 uint8 GRIPPER_ACTION_GRAB = 1
 
+#used as param1 in SET_SAFETY_SWITCH command
+uint8 SAFETY_SWITCH_STATE_SAFE = 0
+uint8 SAFETY_SWITCH_STATE_DANGEROUS = 1
+
 uint8 ORB_QUEUE_LENGTH = 8
 
 float32 param1			# Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.

src/modules/commander/Commander.cpp

@@ -1448,6 +1448,53 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 		break;
 
+	case vehicle_command_s::VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE: {
+
+			const uint8_t set_safety_switch_state = static_cast<uint8_t>(cmd.param1 + 0.5f);
+
+			if (set_safety_switch_state == vehicle_command_s::SAFETY_SWITCH_STATE_DANGEROUS) {
+
+				_safety.deactivateSafety();
+
+				if (_safety.isSafetyOff()) {
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					/* EVENT
+					 * @description Safety switch off via mavlink command
+					 */
+					events::send(events::ID("commander_safety_off"), events::Log::Info, "Safety switch off");
+
+				} else {
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
+				}
+
+			} else if (set_safety_switch_state == vehicle_command_s::SAFETY_SWITCH_STATE_SAFE) {
+
+				_safety.activateSafety();
+
+				if (!_safety.isSafetyOff()) {
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					/* EVENT
+					 * @description Safety switch on via mavlink command
+					 */
+					events::send(events::ID("commander_safety_on"), events::Log::Info, "Safety switch on");
+
+				} else {
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
+				}
+
+			} else {
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
+				/* EVENT
+				 * @description Invalid safety switch param sent via mavlink command
+				 */
+				events::send(events::ID("commander_safety_cmd_fail"), events::Log::Notice,
+					     "Invalid safety switch param sent. Safety switch on");
+			}
+
+		}
+
+		break;
+
 	case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:

src/modules/commander/Safety.cpp

@@ -76,3 +76,10 @@ void Safety::activateSafety()
 		_safety_off = false;
 	}
 }
+
+void Safety::deactivateSafety()
+{
+	if (!_safety_disabled) { // If Safety circuit breaker is disabled, safety can be turned off
+		_safety_off = true; // Safety off i.e. SAFETY_SWITCH_STATE_DANGEROUS
+	}
+}

src/modules/commander/Safety.hpp

@@ -49,6 +49,7 @@ class Safety
 
 	bool safetyButtonHandler();
 	void activateSafety();
+	void deactivateSafety();
 	bool isButtonAvailable() const { return _button_available; }
 	bool isSafetyOff() const { return _safety_off; }
 	bool isSafetyDisabled() const { return _safety_disabled; }