diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index c46780167e..81094d3de7 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -39,7 +39,6 @@ px4_add_module( STACK_MAIN 4096 STACK_MAX 2450 COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS accelerometer_calibration.cpp airspeed_calibration.cpp diff --git a/src/modules/commander/arm_auth.cpp b/src/modules/commander/arm_auth.cpp index 4dbd79b799..5be6cc73b4 100644 --- a/src/modules/commander/arm_auth.cpp +++ b/src/modules/commander/arm_auth.cpp @@ -57,13 +57,16 @@ static enum { 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; +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; @@ -81,7 +84,7 @@ 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.authorizer_system_id; + vcmd.target_system = arm_parameters.struct_value.authorizer_system_id; uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; vcmd_pub.publish(vcmd); @@ -105,7 +108,7 @@ static uint8_t _auth_method_arm_req_check() 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); + auth_timeout = now + (arm_parameters.struct_value.auth_method_param.auth_method_arm_timeout_msec * 1000); state = ARM_AUTH_WAITING_AUTH; while (now < auth_timeout) { @@ -157,7 +160,7 @@ static uint8_t _auth_method_two_arm_check() 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); + auth_timeout = now + (arm_parameters.struct_value.auth_method_param.auth_method_arm_timeout_msec * 1000); state = ARM_AUTH_WAITING_AUTH; mavlink_log_critical(mavlink_log_pub, "Arm auth: Requesting authorization..."); @@ -167,8 +170,8 @@ static uint8_t _auth_method_two_arm_check() uint8_t arm_auth_check() { - if (arm_parameters.authentication_method < ARM_AUTH_METHOD_LAST) { - return arm_check_method[arm_parameters.authentication_method](); + if (arm_parameters.struct_value.authentication_method < ARM_AUTH_METHOD_LAST) { + return arm_check_method[arm_parameters.struct_value.authentication_method](); } return vehicle_command_ack_s::VEHICLE_RESULT_DENIED; @@ -177,7 +180,7 @@ uint8_t arm_auth_check() void arm_auth_update(hrt_abstime now, bool param_update) { if (param_update) { - param_get(param_arm_parameters, (int32_t *)&arm_parameters); + param_get(param_arm_parameters, &arm_parameters.param_value); } switch (state) { @@ -280,5 +283,5 @@ void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *sys_id) enum arm_auth_methods arm_auth_method_get() { - return (enum arm_auth_methods) arm_parameters.authentication_method; + return (enum arm_auth_methods) arm_parameters.struct_value.authentication_method; }