modules: commander: Implement arm authorization request

If the second bit of COM_ARM_MIS_EXT_REQ is set the vehicle
will only arm after receive an authorization.

The authorization flow:
vehicle/external -> command: arm authorization request -> arm authorizer
vehicle <- command ack with result in progress <- arm authorizer
vehicle <- any data request <- arm authorizer
vehicle -> data response -> arm authorizer
vehicle <- command ack authorizing or denying <- arm authorizer

Right now there is 2 ways to start the arm authorization request,
that can be configured by COM_ARM_AUTH parameter.
- One arm: When pilot request the vehicle to arm, it will request
authorization blocking the arm process up to the timeout defined in
COM_ARM_AUTH parameter.
- Two arms request: The first arm request will request the
authorization and will deny the first arm request, if authorizer
approved the request, pilot can arm again within the authorized
time and arm without any block.

The arm authorizer can be running anywhere(compute board or PX4
itself) and it is responsible to request the mission list or any
other information to vehicle before send a final response, it
should send to vehicle a COMMAND_ACK with
result = MAV_RESULT_IN_PROGRESS as soon as it receive the arm
authorization request and the final result
as after it got all the data that it needs authorize or deny the
request.

src/modules/commander/CMakeLists.txt

@@ -49,6 +49,7 @@ px4_add_module(
 		airspeed_calibration.cpp
 		esc_calibration.cpp
 		PreflightCheck.cpp
+		arm_auth.cpp
 	DEPENDS
 		platforms__common
 	)

src/modules/commander/arm_auth.cpp

@@ -0,0 +1,274 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017  Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+#include "arm_auth.h"
+
+#include <string.h>
+#include <unistd.h>
+
+#include <px4_defines.h>
+#include <px4_config.h>
+
+#include <systemlib/mavlink_log.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+
+static orb_advert_t handle_vehicle_command_pub;
+static orb_advert_t *mavlink_log_pub;
+static int command_ack_sub = -1;
+
+static param_t param_arm_parameters;
+
+static hrt_abstime auth_timeout;
+
+static enum {
+	ARM_AUTH_IDLE = 0,
+	ARM_AUTH_WAITING_AUTH,
+	ARM_AUTH_WAITING_AUTH_WITH_ACK,
+	ARM_AUTH_MISSION_APPROVED
+} state = ARM_AUTH_IDLE;
+
+struct packed_struct {
+	uint8_t authorizer_system_id;
+	union {
+		uint16_t auth_method_arm_timeout_msec;
+		uint16_t auth_method_two_arm_timeout_msec;
+	} auth_method_param;
+	uint8_t authentication_method;
+} arm_parameters;
+
+static uint32_t *system_id;
+
+static uint8_t _auth_method_arm_req_check();
+static uint8_t _auth_method_two_arm_check();
+
+static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = {
+	_auth_method_arm_req_check,
+	_auth_method_two_arm_check,
+};
+
+static void arm_auth_request_msg_send()
+{
+	struct vehicle_command_s cmd = {
+		.timestamp = 0,
+		.param5 = 0,
+		.param6 = 0,
+		.param1 = 0,
+		.param2 = 0,
+		.param3 = 0,
+		.param4 = 0,
+		.param7 = 0,
+		.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST,
+		.target_system = arm_parameters.authorizer_system_id
+	};
+
+	if (handle_vehicle_command_pub == nullptr) {
+		handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
+	} else {
+		orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &cmd);
+	}
+}
+
+static uint8_t _auth_method_arm_req_check()
+{
+	switch (state) {
+	case ARM_AUTH_IDLE:
+		/* no authentication in process? handle bellow */
+		break;
+	case ARM_AUTH_MISSION_APPROVED:
+		return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+	default:
+		return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+	}
+
+	/* handling ARM_AUTH_IDLE */
+	arm_auth_request_msg_send();
+
+	hrt_abstime now = hrt_absolute_time();
+	auth_timeout = now + (arm_parameters.auth_method_param.auth_method_arm_timeout_msec * 1000);
+	state = ARM_AUTH_WAITING_AUTH;
+
+	while (now < auth_timeout) {
+		arm_auth_update(now);
+
+		if (state != ARM_AUTH_WAITING_AUTH && state != ARM_AUTH_WAITING_AUTH_WITH_ACK) {
+			break;
+		}
+
+		/* 0.5ms */
+		usleep(500);
+		now = hrt_absolute_time();
+	}
+
+	switch (state) {
+	case ARM_AUTH_WAITING_AUTH:
+	case ARM_AUTH_WAITING_AUTH_WITH_ACK:
+		state = ARM_AUTH_IDLE;
+		mavlink_log_critical(mavlink_log_pub, "Arm auth: No response");
+		break;
+	default:
+		break;
+	}
+
+	return state == ARM_AUTH_MISSION_APPROVED ?
+			vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+}
+
+static uint8_t _auth_method_two_arm_check()
+{
+	switch (state) {
+	case ARM_AUTH_IDLE:
+		/* no authentication in process? handle bellow */
+		break;
+	case ARM_AUTH_MISSION_APPROVED:
+		return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+	case ARM_AUTH_WAITING_AUTH:
+	case ARM_AUTH_WAITING_AUTH_WITH_ACK:
+		return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
+	default:
+		return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+	}
+
+	/* handling ARM_AUTH_IDLE */
+	arm_auth_request_msg_send();
+
+	hrt_abstime now = hrt_absolute_time();
+	auth_timeout = now + (arm_parameters.auth_method_param.auth_method_arm_timeout_msec * 1000);
+	state = ARM_AUTH_WAITING_AUTH;
+
+	mavlink_log_critical(mavlink_log_pub, "Arm auth: Requesting authorization...");
+
+	return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
+}
+
+uint8_t arm_auth_check()
+{
+	if (arm_parameters.authentication_method < ARM_AUTH_METHOD_LAST) {
+		return arm_check_method[arm_parameters.authentication_method]();
+	}
+
+	return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+}
+
+void arm_auth_update(hrt_abstime now)
+{
+	param_get(param_arm_parameters, &arm_parameters);
+
+	switch (state) {
+	case ARM_AUTH_WAITING_AUTH:
+	case ARM_AUTH_WAITING_AUTH_WITH_ACK:
+		/* handle bellow */
+		break;
+	case ARM_AUTH_MISSION_APPROVED:
+		if (now > auth_timeout) {
+			state = ARM_AUTH_IDLE;
+		}
+		return;
+	case ARM_AUTH_IDLE:
+	default:
+		return;
+	}
+
+	/*
+	 * handling ARM_AUTH_WAITING_AUTH, ARM_AUTH_WAITING_AUTH_WITH_ACK
+	 */
+	vehicle_command_ack_s command_ack;
+	bool updated = false;
+
+	orb_check(command_ack_sub, &updated);
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_command_ack), command_ack_sub, &command_ack);
+	}
+
+	if (updated
+			&& command_ack.command == vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST
+			&& command_ack.target_system == *system_id) {
+		switch (command_ack.result) {
+		case vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS:
+			state = ARM_AUTH_WAITING_AUTH_WITH_ACK;
+			break;
+		case vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED:
+			mavlink_log_critical(mavlink_log_pub,
+					"Arm auth: Authorized for the next %u seconds",
+					command_ack.result_param2);
+			state = ARM_AUTH_MISSION_APPROVED;
+			auth_timeout = now + (command_ack.result_param2 * 1000000);
+			return;
+		case vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED:
+			mavlink_log_critical(mavlink_log_pub, "Arm auth: Temporarily rejected");
+			state = ARM_AUTH_IDLE;
+			return;
+		case vehicle_command_ack_s::VEHICLE_RESULT_DENIED:
+		default:
+			switch (command_ack.result_param1) {
+			case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_NONE:
+				/* Authorizer will send reason to ground station */
+				break;
+			case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT:
+				mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied, waypoint %i have a invalid value", command_ack.result_param2);
+				break;
+			case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_TIMEOUT:
+				mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied by timeout in authorizer");
+				break;
+			case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE:
+				mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because airspace is in use");
+				break;
+			case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_BAD_WEATHER:
+				mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because of bad weather");
+				break;
+			case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_GENERIC:
+			default:
+				mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied");
+			}
+			state = ARM_AUTH_IDLE;
+			return;
+		}
+	}
+
+	if (now > auth_timeout) {
+		mavlink_log_critical(mavlink_log_pub, "Arm auth: No response");
+		state = ARM_AUTH_IDLE;
+	}
+}
+
+void arm_auth_init(orb_advert_t *mav_log_pub, uint32_t *sys_id)
+{
+	system_id = sys_id;
+	param_arm_parameters = param_find("COM_ARM_AUTH");
+	command_ack_sub = orb_subscribe(ORB_ID(vehicle_command_ack));
+	mavlink_log_pub = mav_log_pub;
+}
+
+enum arm_auth_methods arm_auth_method_get()
+{
+	return (enum arm_auth_methods) arm_parameters.authentication_method;
+}

src/modules/commander/arm_auth.h

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017  Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <stdint.h>
+
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+
+enum arm_auth_methods {
+	ARM_AUTH_METHOD_ARM_REQ = 0,
+	ARM_AUTH_METHOD_TWO_ARM_REQ,
+	ARM_AUTH_METHOD_LAST
+};
+
+void arm_auth_init(orb_advert_t *mav_log_pub, uint32_t *system_id);
+void arm_auth_update(hrt_abstime now);
+uint8_t arm_auth_check();
+enum arm_auth_methods arm_auth_method_get();

src/modules/commander/commander.cpp

@@ -55,6 +55,7 @@
 #include "esc_calibration.h"
 #include "gyro_calibration.h"
 #include "mag_calibration.h"
+#include "arm_auth.h"
 #include "PreflightCheck.h"
 #include "px4_custom_mode.h"
 #include "rc_calibration.h"
@@ -1802,6 +1803,8 @@ int commander_thread_main(int argc, char *argv[])
 	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, nullptr);
 	pthread_attr_destroy(&commander_low_prio_attr);
 
+	arm_auth_init(&mavlink_log_pub, &(status.system_id));
+
 	while (!thread_should_exit) {
 
 		arming_ret = TRANSITION_NOT_CHANGED;
@@ -3217,6 +3220,8 @@ int commander_thread_main(int argc, char *argv[])
 			commander_state_pub = orb_advertise(ORB_ID(commander_state), &internal_state);
 		}
 
+		arm_auth_update(now);
+
 		usleep(COMMANDER_MONITORING_INTERVAL);
 	}
 

src/modules/commander/commander_params.c

@@ -620,3 +620,20 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0);
  * @group Mission
  */
 PARAM_DEFINE_INT32(COM_POSCTL_NAVL, 0);
+
+/**
+ * Arm authorization parameters, this uint32_t will be splitted between starting from the LSB:
+ * - 8bits to authorizer system id
+ * - 16bits to authentication method parameter, this will be used to store a timeout for the first 2 methods but can be used to another parameter for other new authentication methods.
+ * - 7bits to authentication method
+ * 		- one arm = 0
+ * 		- two step arm = 1
+ * * the MSB bit is not used to avoid problems in the conversion between int and uint
+ *
+ * Default value: (10 << 0 | 1000 << 8 | 0 << 24) = 256010
+ * - authorizer system id = 10
+ * - authentication method parameter = 10000msec of timeout
+ * - authentication method = during arm
+ * @group Commander
+ */
+PARAM_DEFINE_INT32(COM_ARM_AUTH, 256010);

src/modules/commander/state_machine_helper.cpp

@@ -47,6 +47,8 @@
 #include <px4_shutdown.h>
 
 #include <uORB/uORB.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
 #include <systemlib/systemlib.h>
@@ -59,6 +61,7 @@
 #include "state_machine_helper.h"
 #include "commander_helper.h"
 #include "PreflightCheck.h"
+#include "arm_auth.h"
 
 #ifndef __PX4_NUTTX
 #include "DevMgr.hpp"
@@ -312,6 +315,15 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
 			}
 		}
 
+		if ((arm_requirements & ARM_REQ_ARM_AUTH_BIT)
+				&& new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
+				&& valid_transition) {
+			if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) {
+				feedback_provided = true;
+				valid_transition = false;
+			}
+		}
+
 		// Finish up the state transition
 		if (valid_transition) {
 			armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED

src/modules/logger/logger.cpp

@@ -565,6 +565,8 @@ void Logger::add_common_topics()
 	add_topic("vehicle_attitude_setpoint", 30);
 	add_topic("vehicle_command");
 	add_topic("vehicle_global_position", 200);
+	add_topic("arm_auth_request");
+	add_topic("arm_auth_ack");
 	add_topic("vehicle_gps_position");
 	add_topic("vehicle_land_detected");
 	add_topic("vehicle_local_position", 100);