From 7f95c6486f0ba93ee3e9ad41fc21a835d854053d Mon Sep 17 00:00:00 2001 From: Junwoo Hwang Date: Thu, 29 Sep 2022 19:38:50 +0200 Subject: [PATCH] Fix wrong GRIPPER_ACTION conversion from floating point to int32_t - Due to the MAVLink spec, we actually just convert enums into floating point, so in PX4 we need to convert the float directly into integer as well (although there can be precision issues on large numbers) - This is a limitation in MAVLink spec, and should hopefully be changed in MAVLink v2 --- src/modules/payload_deliverer/payload_deliverer.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/payload_deliverer/payload_deliverer.cpp b/src/modules/payload_deliverer/payload_deliverer.cpp index c44995fdac..5be494b0e5 100644 --- a/src/modules/payload_deliverer/payload_deliverer.cpp +++ b/src/modules/payload_deliverer/payload_deliverer.cpp @@ -158,7 +158,7 @@ void PayloadDeliverer::handle_vehicle_command(const hrt_abstime &now, const veh vehicle_command_ack_s vcmd_ack{}; vcmd_ack.timestamp = now; vcmd_ack.command = vehicle_command_s::VEHICLE_CMD_DO_GRIPPER; - vcmd_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; + vcmd_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; vcmd_ack.target_system = vehicle_command->source_system; vcmd_ack.target_component = vehicle_command->source_component; _vehicle_command_ack_pub.publish(vcmd_ack); @@ -166,7 +166,10 @@ void PayloadDeliverer::handle_vehicle_command(const hrt_abstime &now, const veh return; } - const int32_t gripper_action = *(int32_t *)&vehicle_command->param2; // Convert the action to integer + const int32_t gripper_action = (int32_t)vehicle_command->param2; + // Note: although the 'param2' is a floating point type, the enums for GRIPPER_ACTION are converted into + // floating point, then gets sent by the Ground Control Station to trigger gripper action for example. + // This is an inherent MAVLink standard limitation (converting enums into floats)! switch (gripper_action) { case vehicle_command_s::GRIPPER_ACTION_GRAB: @@ -183,7 +186,7 @@ void PayloadDeliverer::handle_vehicle_command(const hrt_abstime &now, const veh vehicle_command_ack_s vcmd_ack{}; vcmd_ack.timestamp = now; vcmd_ack.command = vehicle_command_s::VEHICLE_CMD_DO_GRIPPER; - vcmd_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS; + vcmd_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS; vcmd_ack.result_param1 = UINT8_MAX; // Set progress percentage to UINT8_MAX, indicating it is unkonwn vcmd_ack.target_system = vehicle_command->source_system; vcmd_ack.target_component = vehicle_command->source_component; @@ -197,7 +200,8 @@ bool PayloadDeliverer::send_gripper_vehicle_command(const int32_t gripper_action vehicle_command_s vcmd; vcmd.timestamp = hrt_absolute_time(); vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GRIPPER; - *(int32_t *)&vcmd.param2 = gripper_action; + vcmd.param2 = gripper_action; + // Note: Integer type GRIPPER_ACTION gets formatted into a floating point here. return _vehicle_command_pub.publish(vcmd); }