From 8b3c78a0a4d1789319a932ed6983054e9797e8ea Mon Sep 17 00:00:00 2001 From: Stefano Colli Date: Thu, 14 Nov 2024 16:23:44 +0100 Subject: [PATCH] Navigator: add optional delay after gimbal mission items --- src/modules/navigator/mission_block.cpp | 26 ++++++++++++++++++++++-- src/modules/navigator/mission_block.h | 17 ++++++++++++++++ src/modules/navigator/mission_params.c | 13 ++++++++++++ src/modules/navigator/navigator.h | 11 +++++----- src/modules/navigator/navigator_main.cpp | 3 +++ 5 files changed, 63 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 02e7352be2..66f17a9766 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -81,7 +81,6 @@ MissionBlock::is_mission_item_reached_or_completed() case NAV_CMD_DO_CONTROL_VIDEO: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: - case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case NAV_CMD_DO_SET_ROI: case NAV_CMD_DO_SET_ROI_LOCATION: @@ -160,6 +159,19 @@ MissionBlock::is_mission_item_reached_or_completed() return false; } + 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) { + PX4_DEBUG("Waited %.2f seconds for the gimbal to reach the desired orientation, resuming mission!", (double) _gimbal_wait_time); + return true; + + } + + // We are still waiting the desired delay for the gimbal + return false; + } + + case NAV_CMD_DO_GRIPPER: { const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * 1E-6f; @@ -595,6 +607,13 @@ MissionBlock::issue_command(const mission_item_s &item) } } + // Register the Gimbal control command (NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) timestamp so to wait + // for the gimbal to reach the desired position + if (item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) + { + _gimbal_command_time = hrt_absolute_time(); + } + _navigator->publish_vehicle_cmd(&vcmd); } } @@ -619,10 +638,13 @@ MissionBlock::get_time_inside(const mission_item_s &item) const // and shouldn't have a timeout defined as it is a DO_* command. It should rather be defined as CONDITION_GRIPPER // or so, and have a function named 'item_is_conditional' // Reference: https://mavlink.io/en/services/mission.html#mavlink_commands +// A similar condition applies to DO_GIMBAL_MANAGER_PITCHYAW bool MissionBlock::item_has_timeout(const mission_item_s &item) { - return item.nav_cmd == NAV_CMD_DO_WINCH || item.nav_cmd == NAV_CMD_DO_GRIPPER; + return item.nav_cmd == NAV_CMD_DO_WINCH || + item.nav_cmd == NAV_CMD_DO_GRIPPER || + item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW; } bool diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 241acdca4f..8628a16cf0 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -133,6 +133,19 @@ public: _payload_deploy_timeout_s = timeout_s; } + /** + * @brief Set the gimbal wait time + * + * Accessed in Navigator to set the appropriate time to wait for the gimbal to reach + * the desired orientation before resuming the mission + * + * @param wait_time Wait time in seconds + */ + void set_gimbal_wait_time(const float wait_time) + { + _gimbal_wait_time = wait_time; + } + /** * Copies position from setpoint if valid, otherwise copies current position */ @@ -252,6 +265,10 @@ protected: hrt_abstime _payload_deployed_time{0}; // Last payload deployment start time to handle timeouts float _payload_deploy_timeout_s{0.0f}; // Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received + /* Gimbal commands time variables are used in case of NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW to wait for the gimbal movement */ + hrt_abstime _gimbal_command_time{0}; // Last gimbal command timestamp + float _gimbal_wait_time{0.0f}; // Time to wait for the gimbal to reach the desired configuration + private: void updateMaxHaglFailsafe(); diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index d02932317f..8420f8253a 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -155,3 +155,16 @@ PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f); * @group Mission */ PARAM_DEFINE_INT32(MIS_LND_ABRT_ALT, 30); + +/** + * Time in seconds allowed for the gimbal to reach its commanded attitude before the mission resumes. + * + * Time allocated for a gimbal command to execute within a mission before resuming. + * This ensures the gimbal has reached the commanded orientation before beginning to take pictures. + * + * @unit s + * @min 0 + * @decimal 1 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_GIM_WAIT_T, 0.0f); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 0da0c72e9f..a33c7cca84 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -424,10 +424,11 @@ private: _param_nav_min_gnd_dist, /**< minimum distance to ground (Mission and RTL)*/ // non-navigator parameters: Mission (MIS_*) - (ParamFloat) _param_mis_takeoff_alt, - (ParamFloat) _param_mis_yaw_tmt, - (ParamFloat) _param_mis_yaw_err, - (ParamFloat) _param_mis_payload_delivery_timeout, - (ParamInt) _param_mis_lnd_abrt_alt + (ParamFloat) _param_mis_takeoff_alt, + (ParamFloat) _param_mis_yaw_tmt, + (ParamFloat) _param_mis_yaw_err, + (ParamFloat) _param_mis_payload_delivery_timeout, + (ParamInt) _param_mis_lnd_abrt_alt, + (ParamFloat) _param_mis_gimbal_wait_t ) }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b1a920a467..482e440ac9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -117,6 +117,7 @@ Navigator::Navigator() : // Update the timeout used in mission_block (which can't hold it's own parameters) _mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get()); + _mission.set_gimbal_wait_time(_param_mis_gimbal_wait_t.get()); _adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(), _param_nav_traff_a_ver.get(), @@ -150,6 +151,8 @@ void Navigator::params_update() } _mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get()); + _mission.set_gimbal_wait_time(_param_mis_gimbal_wait_t.get()); + } void Navigator::run()