diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index cbe474dabe..dda3d4d2ca 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/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 ) diff --git a/src/modules/commander/arm_auth.cpp b/src/modules/commander/arm_auth.cpp new file mode 100644 index 0000000000..dea65d75d2 --- /dev/null +++ b/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 +#include + +#include +#include + +#include +#include +#include + +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; +} diff --git a/src/modules/commander/arm_auth.h b/src/modules/commander/arm_auth.h new file mode 100644 index 0000000000..1d43bbaea1 --- /dev/null +++ b/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 + +#include +#include + +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(); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 99d53ed700..fee2b9ad91 100644 --- a/src/modules/commander/commander.cpp +++ b/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); } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 7479c411a2..4ea0a5c264 100644 --- a/src/modules/commander/commander_params.c +++ b/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); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 276ae8e88a..266c81832d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -47,6 +47,8 @@ #include #include +#include +#include #include #include #include @@ -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 diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 302b550fee..adefdf696c 100644 --- a/src/modules/logger/logger.cpp +++ b/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);