fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 09:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mission_block: simplify timeout check

Browse Source
This commit is contained in:
Matthias Grob
2024-11-28 20:23:36 +01:00
parent ec1cf04bc9
commit 17c24bafbc
1 changed files with 3 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+3 -10
View File
@@ -142,14 +142,11 @@ MissionBlock::is_mission_item_reached_or_completed()
break;
case NAV_CMD_DO_WINCH: {
const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) *
1E-6f; // TODO: Add proper microseconds_to_seconds function
if (_payload_deploy_ack_successful) {
PX4_DEBUG("Winch has acknowledged, resume mission");
return true;
} else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) {
} else if (now > _payload_deployed_time + (_payload_deploy_timeout_s * 1_s)) {
PX4_DEBUG("Winch timed out, resume mission");
return true;
@@ -160,9 +157,7 @@ MissionBlock::is_mission_item_reached_or_completed()
}
case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: {
const float gimbal_command_elasped_time_s = (now - _gimbal_command_time) * 1E-6f;
if (gimbal_command_elasped_time_s > _gimbal_wait_time) {
if (now > _gimbal_command_time + (_gimbal_wait_time * 1_s)) {
PX4_DEBUG("Gimbal aligned, resume mission");
return true;
}
@@ -172,13 +167,11 @@ MissionBlock::is_mission_item_reached_or_completed()
}
case NAV_CMD_DO_GRIPPER: {
const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * 1E-6f;
if (_payload_deploy_ack_successful) {
PX4_DEBUG("Gripper has acknowledged, resume mission");
return true;
} else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) {
} else if (now > _payload_deployed_time + (_payload_deploy_timeout_s * 1_s)) {
PX4_DEBUG("Gripper timed out, resume mission");
return true;