arm auth: split COM_ARM_AUTH in 3 parameters

new params are:
- COM_ARM_AUTH_ID
- COM_ARM_AUTH_MET
- COM_ARM_AUTH_TO
This commit is contained in:
Nicolas MARTIN
2020-09-17 10:46:12 +02:00
committed by Beat Küng
parent 6579544fca
commit 8b96ff57d7
3 changed files with 70 additions and 44 deletions
@@ -43,14 +43,18 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
using namespace time_literals;
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 hrt_abstime auth_req_time;
static hrt_abstime _param_com_arm_auth_timout;
static enum arm_auth_methods _param_com_arm_auth_method;
static int32_t _param_com_arm_auth_id;
static enum {
ARM_AUTH_IDLE = 0,
ARM_AUTH_WAITING_AUTH,
@@ -58,18 +62,6 @@ static enum {
ARM_AUTH_MISSION_APPROVED
} state = ARM_AUTH_IDLE;
union {
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;
} __attribute__((packed)) struct_value;
int32_t param_value;
} arm_parameters;
static uint8_t *system_id;
static uint8_t _auth_method_arm_req_check();
@@ -80,12 +72,31 @@ static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = {
_auth_method_two_arm_check,
};
void arm_auth_param_update()
{
float timeout = 0;
param_get(param_find("COM_ARM_AUTH_TO"), &timeout);
_param_com_arm_auth_timout = timeout * 1_s;
int32_t auth_method = ARM_AUTH_METHOD_ARM_REQ;
param_get(param_find("COM_ARM_AUTH_MET"), &auth_method);
if (auth_method >= 0 && auth_method < ARM_AUTH_METHOD_LAST) {
_param_com_arm_auth_method = (arm_auth_methods)auth_method;
} else {
_param_com_arm_auth_method = ARM_AUTH_METHOD_ARM_REQ;
}
param_get(param_find("COM_ARM_AUTH_ID"), &_param_com_arm_auth_id);
}
static void arm_auth_request_msg_send()
{
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST;
vcmd.target_system = arm_parameters.struct_value.authorizer_system_id;
vcmd.target_system = _param_com_arm_auth_id;
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd_pub.publish(vcmd);
@@ -110,7 +121,7 @@ static uint8_t _auth_method_arm_req_check()
hrt_abstime now = hrt_absolute_time();
auth_req_time = now;
auth_timeout = now + (arm_parameters.struct_value.auth_method_param.auth_method_arm_timeout_msec * 1000);
auth_timeout = now + _param_com_arm_auth_timout;
state = ARM_AUTH_WAITING_AUTH;
while (now < auth_timeout) {
@@ -163,7 +174,7 @@ static uint8_t _auth_method_two_arm_check()
hrt_abstime now = hrt_absolute_time();
auth_req_time = now;
auth_timeout = now + (arm_parameters.struct_value.auth_method_param.auth_method_arm_timeout_msec * 1000);
auth_timeout = now + _param_com_arm_auth_timout;
state = ARM_AUTH_WAITING_AUTH;
mavlink_log_info(mavlink_log_pub, "Arm auth: Requesting authorization...");
@@ -173,8 +184,8 @@ static uint8_t _auth_method_two_arm_check()
uint8_t arm_auth_check()
{
if (arm_parameters.struct_value.authentication_method < ARM_AUTH_METHOD_LAST) {
return arm_check_method[arm_parameters.struct_value.authentication_method]();
if (_param_com_arm_auth_method < ARM_AUTH_METHOD_LAST) {
return arm_check_method[_param_com_arm_auth_method]();
}
return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
@@ -183,7 +194,7 @@ uint8_t arm_auth_check()
void arm_auth_update(hrt_abstime now, bool param_update)
{
if (param_update) {
param_get(param_arm_parameters, &arm_parameters.param_value);
arm_auth_param_update();
}
switch (state) {
@@ -279,13 +290,8 @@ void arm_auth_update(hrt_abstime now, bool param_update)
void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *sys_id)
{
arm_auth_param_update();
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.struct_value.authentication_method;
}
@@ -47,4 +47,3 @@ enum arm_auth_methods {
void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *system_id);
void arm_auth_update(hrt_abstime now, bool param_update = true);
uint8_t arm_auth_check();
enum arm_auth_methods arm_auth_method_get();
+38 -17
View File
@@ -680,23 +680,6 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0);
*/
PARAM_DEFINE_INT32(COM_POSCTL_NAVL, 0);
/**
* Arm authorization parameters, this uint32_t will be split 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 = 1000 msec of timeout
* - authentication method = during arm
* @group Commander
*/
PARAM_DEFINE_INT32(COM_ARM_AUTH, 256010);
/**
* Require arm authorization to arm
*
@@ -707,6 +690,44 @@ PARAM_DEFINE_INT32(COM_ARM_AUTH, 256010);
*/
PARAM_DEFINE_INT32(COM_ARM_AUTH_REQ, 0);
/**
* Arm authorizer system id
*
* Used if arm authorization is requested by COM_ARM_AUTH_REQ.
*
* @group Commander
*/
PARAM_DEFINE_INT32(COM_ARM_AUTH_ID, 10);
/**
* Arm authorization method
*
* Methods:
* - one arm: request authorization and arm when authorization is received
* - two step arm: 1st arm command request an authorization and
* 2nd arm command arm the drone if authorized
*
* Used if arm authorization is requested by COM_ARM_AUTH_REQ.
*
* @group Commander
* @value 0 one arm
* @value 1 two step arm
*/
PARAM_DEFINE_INT32(COM_ARM_AUTH_MET, 0);
/**
* Arm authorization timeout
*
* Timeout for authorizer answer.
* Used if arm authorization is requested by COM_ARM_AUTH_REQ.
*
* @group Commander
* @unit s
* @decimal 1
* @increment 0.1
*/
PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1);
/**
* Loss of position failsafe activation delay.
*