diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1d48e3acbb..b07542984d 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -141,45 +141,14 @@ MissionBlock::is_mission_item_reached_or_completed() break; - case NAV_CMD_DO_WINCH: { - if (_payload_deploy_ack_successful) { - PX4_DEBUG("Winch has acknowledged, resume mission"); - return true; - - } else if (now > _payload_deployed_time + (_payload_deploy_timeout_s * 1_s)) { - PX4_DEBUG("Winch timed out, resume mission"); - return true; - - } - - // We are still waiting for the acknowledgement / execution of deploy - return false; + case NAV_CMD_DO_WINCH: + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case NAV_CMD_DO_GRIPPER: + if (now > _timestamp_command_timeout + (_command_timeout * 1_s)) { + return true; } - case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: { - if (now > _gimbal_command_time + (_gimbal_wait_time * 1_s)) { - PX4_DEBUG("Gimbal aligned, resume mission"); - return true; - } - - // We are still waiting the desired delay for the gimbal - return false; - } - - case NAV_CMD_DO_GRIPPER: { - if (_payload_deploy_ack_successful) { - PX4_DEBUG("Gripper has acknowledged, resume mission"); - return true; - - } else if (now > _payload_deployed_time + (_payload_deploy_timeout_s * 1_s)) { - PX4_DEBUG("Gripper timed out, resume mission"); - return true; - - } - - // We are still waiting for the acknowledgement / execution of deploy - return false; - } + return false; // Still waiting default: /* do nothing, this is a 3D waypoint */ @@ -449,7 +418,7 @@ MissionBlock::is_mission_item_reached_or_completed() /* check if the MAV was long enough inside the waypoint orbit */ if ((get_time_inside(_mission_item) < FLT_EPSILON) || - (now - _time_wp_reached >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) { + (now >= (hrt_abstime)(get_time_inside(_mission_item) * 1_s) + _time_wp_reached)) { time_inside_reached = true; } @@ -553,59 +522,37 @@ MissionBlock::issue_command(const mission_item_s &item) return; } - if (item.nav_cmd == NAV_CMD_DO_WINCH || - item.nav_cmd == NAV_CMD_DO_GRIPPER) { - // Initiate Payload Deployment - vehicle_command_s vcmd = {}; - vcmd.command = item.nav_cmd; - vcmd.param1 = item.params[0]; - vcmd.param2 = item.params[1]; - vcmd.param3 = item.params[2]; - vcmd.param4 = item.params[3]; - vcmd.param5 = static_cast(item.params[4]); - vcmd.param6 = static_cast(item.params[5]); - _navigator->publish_vehicle_cmd(&vcmd); + // This is to support legacy DO_MOUNT_CONTROL as part of a mission. + if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) { + _navigator->acquire_gimbal_control(); + } - // Reset payload deploy flag & data to get ready to receive deployment ack result - _payload_deploy_ack_successful = false; - _payload_deployed_time = hrt_absolute_time(); + // Mission item's NAV_CMD enums directly map to the according vehicle command + // So set the raw value directly (MAV_FRAME_MISSION mission item) + vehicle_command_s vcmd = {}; + vcmd.command = item.nav_cmd; + vcmd.param1 = item.params[0]; + vcmd.param2 = item.params[1]; + vcmd.param3 = item.params[2]; + vcmd.param4 = item.params[3]; + vcmd.param5 = static_cast(item.params[4]); + vcmd.param6 = static_cast(item.params[5]); + vcmd.param7 = item.params[6]; - } else { + if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) { + // We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision + vcmd.param5 = item.lat; + vcmd.param6 = item.lon; - // This is to support legacy DO_MOUNT_CONTROL as part of a mission. - if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) { - _navigator->acquire_gimbal_control(); + if (item.altitude_is_relative) { + vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; } + } - // Mission item's NAV_CMD enums directly map to the according vehicle command - // So set the raw value directly (MAV_FRAME_MISSION mission item) - vehicle_command_s vcmd = {}; - vcmd.command = item.nav_cmd; - vcmd.param1 = item.params[0]; - vcmd.param2 = item.params[1]; - vcmd.param3 = item.params[2]; - vcmd.param4 = item.params[3]; - vcmd.param5 = static_cast(item.params[4]); - vcmd.param6 = static_cast(item.params[5]); - vcmd.param7 = item.params[6]; + _navigator->publish_vehicle_cmd(&vcmd); - if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) { - // We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision - vcmd.param5 = item.lat; - vcmd.param6 = item.lon; - - if (item.altitude_is_relative) { - vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; - } - } - - // 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); + if (item_has_timeout(item)) { + _timestamp_command_timeout = hrt_absolute_time(); } } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 8628a16cf0..52f626a2fe 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -108,43 +108,11 @@ public: static bool item_contains_marker(const mission_item_s &item); /** - * @brief Set the payload deployment successful flag object + * Set the item_has_timeout() command timeout * - * Function is accessed in Navigator (navigator_main.cpp) to flag when a successful - * payload deployment ack command has been received. Which in turn allows the mission item - * to continue to the next in the 'is_mission_item_reached_or_completed' function below + * @param timeout Timeout in seconds */ - void set_payload_depolyment_successful_flag(bool payload_deploy_result) - { - _payload_deploy_ack_successful = payload_deploy_result; - } - - /** - * @brief Set the payload deployment timeout - * - * Accessed in Navigator to set the appropriate timeout to wait for while waiting for a successful - * payload delivery acknowledgement. If the payload deployment takes longer than timeout, mission will - * continue into the next item automatically. - * - * @param timeout_s Timeout in seconds - */ - void set_payload_deployment_timeout(const float timeout_s) - { - _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; - } + void set_command_timeout(const float timeout) { _command_timeout = timeout; } /** * Copies position from setpoint if valid, otherwise copies current position @@ -260,16 +228,10 @@ protected: hrt_abstime _time_wp_reached{0}; - /* Payload Deploy internal states are used by two NAV_CMDs: DO_WINCH and DO_GRIPPER */ - bool _payload_deploy_ack_successful{false}; // Flag to keep track of whether we received an acknowledgement for a successful payload deployment - 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 + // Mission items that have a timeout to allow the payload e.g. gripper, winch, gimbal executing the command see item_has_timeout() + hrt_abstime _timestamp_command_timeout{0}; ///< Timestamp when the current item_has_timeout() command was started + float _command_timeout{0.f}; ///< Time in seconds any item_has_timeout() command should be waited for before continuing the mission private: void updateMaxHaglFailsafe(); - }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 8420f8253a..c0bbf5c254 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -132,17 +132,6 @@ PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f); */ PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f); -/** - * Timeout for a successful payload deployment acknowledgement - * - * @unit s - * @min 0 - * @decimal 1 - * @increment 1 - * @group Mission - */ -PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f); - /** * Landing abort min altitude * @@ -157,14 +146,19 @@ PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f); PARAM_DEFINE_INT32(MIS_LND_ABRT_ALT, 30); /** - * Time in seconds allowed for the gimbal to reach its commanded attitude before the mission resumes. + * Timeout to allow the payload to execute the mission command * - * 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. + * Ensure: + * gripper: NAV_CMD_DO_GRIPPER + * has released before continuing mission. + * winch: CMD_DO_WINCH + * has delivered before continuing mission. + * gimbal: CMD_DO_GIMBAL_MANAGER_PITCHYAW + * 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); +PARAM_DEFINE_FLOAT(MIS_COMMAND_TOUT, 0.f); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index a33c7cca84..b3057e5375 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -427,8 +427,7 @@ private: (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 + (ParamFloat) _param_mis_command_tout ) }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 482e440ac9..5f397786fb 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -115,14 +115,6 @@ Navigator::Navigator() : _distance_sensor_mode_change_request_pub.get().request_on_off = distance_sensor_mode_change_request_s::REQUEST_OFF; _distance_sensor_mode_change_request_pub.update(); - // 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(), - _param_nav_traff_collision_time.get(), _param_nav_traff_avoid.get()); - reset_triplets(); } @@ -150,9 +142,10 @@ void Navigator::params_update() param_get(_handle_mpc_acc_hor, &_param_mpc_acc_hor); } - _mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get()); - _mission.set_gimbal_wait_time(_param_mis_gimbal_wait_t.get()); - + _mission.set_command_timeout(_param_mis_command_tout.get()); + _adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(), + _param_nav_traff_a_ver.get(), + _param_nav_traff_collision_time.get(), _param_nav_traff_avoid.get()); } void Navigator::run() @@ -1270,9 +1263,7 @@ void Navigator::run_fake_traffic() void Navigator::check_traffic() { - if (_traffic_sub.updated()) { - _traffic_sub.copy(&_adsb_conflict._transponder_report); uint16_t required_flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | @@ -1291,7 +1282,6 @@ void Navigator::check_traffic() } _adsb_conflict.remove_expired_conflicts(); - } bool Navigator::abort_landing()