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

Navigator: Add ability to wait for a position gate with executing the mission

Browse Source 
This enables the navigator to wait for a specific gate coordinate to pass orthogonally to the current trajectory. This is particularly helpful for payload / camera handling in missions and avoids a dependency of payload handling on navigation waypoints.
This commit is contained in:
Lorenz Meier
2020-03-14 17:50:40 +01:00
parent ed8cb1c5a7
commit b405c7e9bd
6 changed files with 152 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+71 -22
View File
@@ -588,11 +588,14 @@ Mission::set_mission_items()
/* mission item that comes after current if available */
struct mission_item_s mission_item_next_position;
struct mission_item_s mission_item_next_next_position;
bool has_next_position_item = false;
bool has_next_next_position_item = false;
work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item)) {
if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item,
&mission_item_next_next_position, &has_next_next_position_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_MISSION) {
mavlink_log_info(_navigator->get_mavlink_log_pub(),
@@ -796,9 +799,16 @@ Mission::set_mission_items()
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
/* set position setpoint to target during the transition */
// TODO: if has_next_position_item and use_next set next, or if use_heading set generated
generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
if (has_next_position_item) {
/* got next mission item, update setpoint triplet */
mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->current);
} else {
_mission_item.yaw = _navigator->get_global_position()->yaw;
/* set position setpoint to target during the transition */
generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
}
}
/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
@@ -1033,16 +1043,32 @@ Mission::set_mission_items()
break;
}
}
if (_mission_item.nav_cmd == NAV_CMD_CONDITION_GATE) {
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
}
}
/*********************************** set setpoints and check next *********************************************/
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* set current position setpoint from mission item (is protected against non-position items) */
if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) {
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
if (item_contains_gate(&_mission_item)) {
if (has_next_position_item) {
/* we have a new position item so set previous position setpoint to current */
set_previous_pos_setpoint();
/* got next mission item, update setpoint triplet */
mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->current);
}
} else {
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* set current position setpoint from mission item (is protected against non-position items) */
if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) {
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
}
}
/* only set the previous position item if the current one really changed */
@@ -1071,21 +1097,32 @@ Mission::set_mission_items()
set_current_mission_item();
}
if (_mission_item.autocontinue && get_time_inside(_mission_item) < FLT_EPSILON) {
/* try to process next mission item */
if (has_next_position_item) {
if (item_contains_gate(&_mission_item)) {
if (has_next_next_position_item) {
/* got next mission item, update setpoint triplet */
mission_apply_limitation(mission_item_next_position);
mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next);
} else {
/* next mission item is not available */
pos_sp_triplet->next.valid = false;
}
} else {
/* vehicle will be paused on current waypoint, don't set next item */
pos_sp_triplet->next.valid = false;
if (_mission_item.autocontinue && get_time_inside(_mission_item) < FLT_EPSILON) {
/* try to process next mission item */
if (has_next_position_item) {
/* got next mission item, update setpoint triplet */
mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);
} else {
/* next mission item is not available */
pos_sp_triplet->next.valid = false;
}
} else {
/* vehicle will be paused on current waypoint, don't set next item */
pos_sp_triplet->next.valid = false;
}
}
/* Save the distance between the current sp and the previous one */
@@ -1430,8 +1467,9 @@ Mission::do_abort_landing()
}
bool
Mission::prepare_mission_items(mission_item_s *mission_item,
mission_item_s *next_position_mission_item, bool *has_next_position_item)
Mission::prepare_mission_items(struct mission_item_s *mission_item,
struct mission_item_s *next_position_mission_item, bool *has_next_position_item,
struct mission_item_s *next_next_position_mission_item, bool *has_next_next_position_item)
{
*has_next_position_item = false;
bool first_res = false;
@@ -1447,18 +1485,29 @@ Mission::prepare_mission_items(mission_item_s *mission_item,
/* trying to find next position mission item */
while (read_mission_item(offset, next_position_mission_item)) {
if (item_contains_position(*next_position_mission_item)) {
*has_next_position_item = true;
break;
}
if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
offset--;
} else {
offset++;
}
if (item_contains_position(*next_position_mission_item)) {
*has_next_position_item = true;
break;
}
}
if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE &&
next_next_position_mission_item && has_next_next_position_item) {
/* trying to find next next position mission item */
while (read_mission_item(onboard, offset, next_next_position_mission_item)) {
offset++;
if (item_contains_position(next_next_position_mission_item)) {
*has_next_next_position_item = true;
break;
}
}
}
}