fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 06:17:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator: unify timeout waiting for payload to execute mission item command

Browse Source 
Used for winch, gripper, gimbal to reach the desired state before continuing the mission.
Ideally we'd have feedback from all these components and not just a feed-forward delay.
This commit is contained in:
Matthias Grob
2024-11-18 16:30:05 +01:00
parent 17c24bafbc
commit ce3fcd503f
5 changed files with 52 additions and 160 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+32 -85
View File
@@ -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<double>(item.params[4]);
vcmd.param6 = static_cast<double>(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<double>(item.params[4]);
vcmd.param6 = static_cast<double>(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<double>(item.params[4]);
vcmd.param6 = static_cast<double>(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();
}
}
src/modules/navigator/mission_block.h
+6 -44
View File
@@ -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();
};
src/modules/navigator/mission_params.c
+9 -15
View File
@@ -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);
src/modules/navigator/navigator.h
+1 -2
View File
@@ -427,8 +427,7 @@ private:
(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
(ParamFloat<px4::params::MIS_YAW_TMT>) _param_mis_yaw_tmt,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err,
(ParamFloat<px4::params::MIS_PD_TO>) _param_mis_payload_delivery_timeout,
(ParamInt<px4::params::MIS_LND_ABRT_ALT>) _param_mis_lnd_abrt_alt,
(ParamFloat<px4::params::MIS_GIM_WAIT_T>) _param_mis_gimbal_wait_t
(ParamFloat<px4::params::MIS_COMMAND_TOUT>) _param_mis_command_tout
)
};
src/modules/navigator/navigator_main.cpp
+4 -14
View File
@@ -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()