From 1425d2f99e82b79d8cd36f39c4ffc4f54abc521c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Mon, 8 May 2017 18:30:47 -0700 Subject: [PATCH 1/4] modules: commander: Group arm requirements in just one byte and add a new requirement Instead of having several bools to each requirement to arm, lets group then in a byte and use bitmask. This also add a new arm requirement "arm authorization" that will be implemented in another patch. --- src/modules/commander/commander.cpp | 41 ++++++++----------- .../state_machine_helper_test.cpp | 3 +- .../commander/state_machine_helper.cpp | 15 +++---- src/modules/commander/state_machine_helper.h | 12 ++++-- 4 files changed, 34 insertions(+), 37 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bf7304c7fbdf..99d53ed70011 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -251,8 +251,7 @@ static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was static float avionics_power_rail_voltage; // voltage of the avionics power rail -static bool arm_without_gps = false; -static bool arm_mission_required = false; +static uint8_t arm_requirements = ARM_REQ_NONE; static bool _last_condition_global_position_valid = false; @@ -468,10 +467,10 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int checkres = 0; - checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, true, false, hrt_elapsed_time(&commander_boot_timestamp)); + checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, ARM_REQ_GPS_BIT, hrt_elapsed_time(&commander_boot_timestamp)); warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); - checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, arm_without_gps, arm_mission_required, hrt_elapsed_time(&commander_boot_timestamp)); + checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; @@ -742,8 +741,7 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co mavlink_log_pub_local, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_res == TRANSITION_CHANGED) { @@ -1728,11 +1726,11 @@ int commander_thread_main(int argc, char *argv[]) int32_t arm_without_gps_param = 0; param_get(_param_arm_without_gps, &arm_without_gps_param); - arm_without_gps = (arm_without_gps_param == 1); + arm_requirements = arm_without_gps_param == 1 ? ARM_REQ_NONE : ARM_REQ_GPS_BIT; int32_t arm_mission_required_param = 0; param_get(_param_arm_mission_required, &arm_mission_required_param); - arm_mission_required = (arm_mission_required_param == 1); + arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT)); status.rc_input_mode = rc_in_off; if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { @@ -1880,10 +1878,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_arm_switch_is_button, &arm_switch_is_button); param_get(_param_arm_without_gps, &arm_without_gps_param); - arm_without_gps = (arm_without_gps_param == 1); - + arm_requirements = arm_without_gps_param == 1 ? ARM_REQ_NONE : ARM_REQ_GPS_BIT; param_get(_param_arm_mission_required, &arm_mission_required_param); - arm_mission_required = (arm_mission_required_param == 1); + arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT)); /* Autostart id */ param_get(_param_autostart_id, &autostart_id); @@ -2009,7 +2006,7 @@ int commander_thread_main(int argc, char *argv[]) } else { /* check sensors also */ (void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed, - (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !arm_without_gps, + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, /* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp)); } @@ -2125,8 +2122,7 @@ int commander_thread_main(int argc, char *argv[]) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))) { arming_state_changed = true; } @@ -2437,8 +2433,7 @@ int commander_thread_main(int argc, char *argv[]) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret == TRANSITION_CHANGED) { @@ -2734,8 +2729,7 @@ int commander_thread_main(int argc, char *argv[]) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret == TRANSITION_CHANGED) { @@ -2789,8 +2783,7 @@ int commander_thread_main(int argc, char *argv[]) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret == TRANSITION_CHANGED) { @@ -4234,8 +4227,7 @@ void *commander_low_prio_loop(void *arg) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); @@ -4330,7 +4322,7 @@ void *commander_low_prio_loop(void *arg) } status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed, - !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !arm_without_gps, + !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), arm_requirements & ARM_REQ_GPS_BIT, /* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp)); arming_state_transition(&status, @@ -4342,8 +4334,7 @@ void *commander_low_prio_loop(void *arg) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); } else { diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index f69842c51793..d262a71c845f 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -297,8 +297,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() nullptr /* no mavlink_log_pub */, &status_flags, 5.0f, /* avionics rail voltage */ - check_gps, - false, /* valid mission not required*/ + (check_gps ? ARM_REQ_GPS_BIT : 0), 2e6 /* 2 seconds after boot, everything should be checked */ ); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9a0f0b7ec766..276ae8e88a9e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -124,8 +124,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log status_flags_s *status_flags, float avionics_power_rail_voltage, - bool arm_without_gps, - bool arm_mission_required, + uint8_t arm_requirements, hrt_abstime time_since_boot) { // Double check that our static arrays are still valid @@ -153,7 +152,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */, - status_flags, battery, arm_without_gps, arm_mission_required, time_since_boot); + status_flags, battery, arm_requirements, + time_since_boot); } /* re-run the pre-flight check as long as sensors are failing */ @@ -164,7 +164,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) { prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */, false /* force_report */, - status_flags, battery, arm_without_gps, arm_mission_required, time_since_boot); + status_flags, battery, arm_requirements, + time_since_boot); status_flags->condition_system_sensors_initialized = (prearm_ret == OK); last_preflight_check = hrt_absolute_time(); last_prearm_ret = prearm_ret; @@ -1029,7 +1030,7 @@ void reset_link_loss_globals(struct actuator_armed_s *armed, const bool old_fail } int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, - status_flags_s *status_flags, battery_status_s *battery, bool arm_without_gps, bool arm_mission_required, + status_flags_s *status_flags, battery_status_s *battery, uint8_t arm_requirements, hrt_abstime time_since_boot) { bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported && @@ -1047,7 +1048,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, sensor_checks, sensor_checks, sensor_checks, sensor_checks, checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), - !arm_without_gps, true, status->is_vtol, reportFailures, prearm, time_since_boot); + arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, reportFailures, prearm, time_since_boot); if (!status_flags->circuit_breaker_engaged_usb_check && status_flags->usb_connected && prearm) { preflight_ok = false; @@ -1066,7 +1067,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p } // mission required - if (arm_mission_required && + if ((arm_requirements & ARM_REQ_MISSION_BIT) && (!status_flags->condition_auto_mission_available || !status_flags->condition_home_position_valid || !status_flags->condition_global_position_valid)) { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 07bf46608a91..1938bcddf771 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,6 +67,13 @@ enum class link_loss_actions_t { LOCKDOWN = 6, }; +typedef enum { + ARM_REQ_NONE = 0, + ARM_REQ_MISSION_BIT = (1 << 0), + ARM_REQ_ARM_AUTH_BIT = (1 << 1), + ARM_REQ_GPS_BIT = (1 << 2), +} arm_requirements_t; + // This is a struct used by the commander internally. struct status_flags_s { bool condition_calibration_enabled; @@ -118,8 +125,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log status_flags_s *status_flags, float avionics_power_rail_voltage, - bool arm_without_gps, - bool arm_mission_required, + uint8_t arm_requirements, hrt_abstime time_since_boot); transition_result_t @@ -164,6 +170,6 @@ void set_data_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *ar int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery, - bool arm_without_gps, bool arm_mission_required, hrt_abstime time_since_boot); + uint8_t arm_requirements, hrt_abstime time_since_boot); #endif /* STATE_MACHINE_HELPER_H_ */ From a51a6a40be3e4bfac20b0883cf69c9e93e9c93b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Mon, 26 Jun 2017 15:31:53 -0700 Subject: [PATCH 2/4] msg: Add arm authorization request command id --- msg/vehicle_command.msg | 1 + msg/vehicle_command_ack.msg | 7 +++++++ 2 files changed, 8 insertions(+) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 0168ab295c4b..bc155aadb10f 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -62,6 +62,7 @@ uint32 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| uint32 VEHICLE_CMD_SET_CAMERA_MODE = 530 # Set camera capture mode (photo, video, etc.) uint32 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition +uint32 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001 # Request arm authorization uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment uint32 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started diff --git a/msg/vehicle_command_ack.msg b/msg/vehicle_command_ack.msg index a6f3f3fa32b1..a416074241b7 100644 --- a/msg/vehicle_command_ack.msg +++ b/msg/vehicle_command_ack.msg @@ -5,6 +5,13 @@ uint8 VEHICLE_RESULT_UNSUPPORTED = 3 uint8 VEHICLE_RESULT_FAILED = 4 uint8 VEHICLE_RESULT_IN_PROGRESS = 5 +uint16 ARM_AUTH_DENIED_REASON_GENERIC = 0 +uint16 ARM_AUTH_DENIED_REASON_NONE = 1 +uint16 ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT = 2 +uint16 ARM_AUTH_DENIED_REASON_TIMEOUT = 3 +uint16 ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4 +uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5 + uint32 ORB_QUEUE_LENGTH = 3 uint16 command From de2cd47bc60c38230eeee57bfc89792ab27024b8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Thu, 13 Jul 2017 16:37:28 -0700 Subject: [PATCH 3/4] mavlink: Do not miss a vehicle_command If 2 or more vehicle_command are queued a call to update() will return the oldest vehicle_command and set the _cmd_time to the timestamp of the last vehicle_command queued losing it. Using update_if_changed() fix this causing all item being consumed one at each call of send(). --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index eef12c3e870f..7da967948de5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -461,7 +461,7 @@ class MavlinkStreamCommandLong : public MavlinkStream struct vehicle_command_s cmd; bool sent = false; - if (_cmd_sub->update(&_cmd_time, &cmd)) { + if (_cmd_sub->update_if_changed(&cmd)) { if (!cmd.from_external) { if (_mavlink->verbose()) { From 05aed72e38c2ed552f8f792c24d302810437cd4b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Mon, 8 May 2017 18:33:58 -0700 Subject: [PATCH 4/4] 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 | 1 + src/modules/commander/arm_auth.cpp | 274 ++++++++++++++++++ src/modules/commander/arm_auth.h | 48 +++ src/modules/commander/commander.cpp | 5 + src/modules/commander/commander_params.c | 17 ++ .../commander/state_machine_helper.cpp | 12 + src/modules/logger/logger.cpp | 2 + 7 files changed, 359 insertions(+) create mode 100644 src/modules/commander/arm_auth.cpp create mode 100644 src/modules/commander/arm_auth.h diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index cbe474dabe09..dda3d4d2ca22 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 000000000000..dea65d75d2de --- /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 000000000000..1d43bbaea1a3 --- /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 99d53ed70011..fee2b9ad9197 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 7479c411a21f..4ea0a5c26499 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 276ae8e88a9e..266c81832d62 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 302b550feec3..adefdf696c09 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);