diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 917541f915..0611162c3f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -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; + } } } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 5c78b688b3..1c39fbc886 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -177,7 +177,8 @@ private: * @return true if current mission item available */ bool prepare_mission_items(mission_item_s *mission_item, - mission_item_s *next_position_mission_item, bool *has_next_position_item); + mission_item_s *next_position_mission_item, bool *has_next_position_item, + mission_item_s *next_next_position_mission_item = nullptr, bool *has_next_next_position_item = nullptr); /** * Read current (offset == 0) or a specific (offset > 0) mission item diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 2527c941a7..f1562bfd37 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -282,6 +282,47 @@ MissionBlock::is_mission_item_reached() } } + } else if (_mission_item.nav_cmd == NAV_CMD_CONDITION_GATE) { + + // default to true as a number of geometric corner cases + // might prevent reaching the gate - we only set this to false + // if we are able to show that the gate conditions are feasible + bool gate_reached = true; + + struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; + + // if either is not valid the gate is reached + if (curr_sp->valid) { + + // zero is the current gate position + matrix::Vector2f gate_sp(0, 0); + struct map_projection_reference_s ref_pos; + map_projection_init(&ref_pos, _mission_item.lat, _mission_item.lon); + + matrix::Vector2f gate_to_curr_sp; + map_projection_project(&ref_pos, curr_sp->lat, curr_sp->lon, &gate_to_curr_sp(0), &gate_to_curr_sp(1)); + matrix::Vector2f vehicle_pos; + map_projection_project(&ref_pos, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + &vehicle_pos(0), &vehicle_pos(1)); + float res = vehicle_pos.dot(gate_to_curr_sp.normalized()); + + // if the dot product (projected vector) is positive, then + // the current position is between the gate position and the + // next waypoint + if (res >= 0) { + gate_reached = true; + + } else { + gate_reached = false; + } + } + + if (gate_reached) { + _waypoint_position_reached = true; + _waypoint_yaw_reached = true; + _time_wp_reached = now; + } + } else if (_mission_item.nav_cmd == NAV_CMD_DELAY) { _waypoint_position_reached = true; _waypoint_yaw_reached = true; @@ -428,12 +469,9 @@ MissionBlock::reset_mission_item_reached() void MissionBlock::issue_command(const mission_item_s &item) { - if (item_contains_position(item)) { - return; - } - - // NAV_CMD_DO_LAND_START is only a marker - if (item.nav_cmd == NAV_CMD_DO_LAND_START) { + if (item_contains_position(item) + || item_contains_gate(item) + || item_contains_marker(item)) { return; } @@ -509,7 +547,19 @@ MissionBlock::item_contains_position(const mission_item_s &item) } bool -MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp) +MissionBlock::item_contains_gate(const struct mission_item_s *item) +{ + return item->nav_cmd == NAV_CMD_CONDITION_GATE; +} + +bool +MissionBlock::item_contains_marker(const struct mission_item_s *item) +{ + return item->nav_cmd == NAV_CMD_DO_LAND_START; +} + +bool +MissionBlock::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) { /* don't change the setpoint for non-position items */ if (!item_contains_position(item)) { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index e6ca6a5feb..7745d20b94 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -67,7 +67,26 @@ public: MissionBlock(const MissionBlock &) = delete; MissionBlock &operator=(const MissionBlock &) = delete; - static bool item_contains_position(const mission_item_s &item); + /** + * Check if the mission item contains a navigation position + * + * @return false if mission item should not be part of the trajectory + */ + static bool item_contains_position(const mission_item_s *item); + + /** + * Check if the mission item contains a gate condition + * + * @return true if mission item is neither a position nor a command + */ + static bool item_contains_gate(const struct mission_item_s *item); + + /** + * Check if the mission item contains a marker + * + * @return true if mission item is neither a position nor a command + */ + static bool item_contains_marker(const struct mission_item_s *item); protected: /** diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 4e302173da..e43eb4b475 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -242,6 +242,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF && missionitem.nav_cmd != NAV_CMD_VTOL_LAND && missionitem.nav_cmd != NAV_CMD_DELAY && + missionitem.nav_cmd != NAV_CMD_CONDITION_GATE && missionitem.nav_cmd != NAV_CMD_DO_JUMP && missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && missionitem.nav_cmd != NAV_CMD_DO_SET_HOME && diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index d1011e489f..882cf1cb4e 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -94,6 +94,7 @@ enum NAV_CMD { NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION = 5002, NAV_CMD_FENCE_CIRCLE_INCLUSION = 5003, NAV_CMD_FENCE_CIRCLE_EXCLUSION = 5004, + NAV_CMD_CONDITION_GATE = 4501, NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */ };