diff options
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 140 |
1 files changed, 85 insertions, 55 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ae7c1f252..79e93f490 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -704,81 +704,112 @@ Navigator::change_current_mission_item(int new_mission_item_index) if (new_mission_item_index == _current_mission_item_index) { return; } - /* or maybe there are no more missions */ + /* or maybe there are no more mission items */ if (new_mission_item_index >= _mission_item_count) { - /* XXX switch to loiter here */ - return; - } - - /* accept new mission item */ - _current_mission_item_index = new_mission_item_index; + + /* just keep the last mission item and set it to loiter */ + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - /* reset all states */ - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - _time_first_inside_orbit = 0; + switch (_mission_item_triplet.current.nav_cmd) { - /* TODO: extend this to different frames, global for now */ + /* if the last mission is not a loiter item, set it to one */ + case NAV_CMD_WAYPOINT: + case NAV_CMD_RETURN_TO_LAUNCH: + case NAV_CMD_TAKEOFF: - _mission_item_triplet.current_valid = false; + _mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item_triplet.current.loiter_radius = 100.0f; + //_mission_item_triplet.current.loiter_radius = _nav_caps.turn_distance; // TODO: publish capabilities somewhere + _mission_item_triplet.current.loiter_direction = 1; + break; - if (_mission_item_count > 0 && new_mission_item_index < _mission_item_count) { - _mission_item_triplet.current_valid = true; - memcpy(&_mission_item_triplet.current, &_mission_item[new_mission_item_index], sizeof(mission_item_s)); - warnx("current is valid"); - } + /* if the last mission item was to loiter, continue unlimited */ + case NAV_CMD_LOITER_TURN_COUNT: + case NAV_CMD_LOITER_TIME_LIMIT: - int previous_setpoint_index = -1; - _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + break; + /* if already in loiter, don't change anything */ + case NAV_CMD_LOITER_UNLIMITED: + break; + /* if landed, stay in land mode */ + case NAV_CMD_LAND: + break; - if (new_mission_item_index > 0) { - previous_setpoint_index = new_mission_item_index - 1; + default: + warnx("Unsupported nav_cmd"); + break; + } } + else { + /* accept new mission item */ + _current_mission_item_index = new_mission_item_index; - while (previous_setpoint_index >= 0) { + /* reset all states */ + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + _time_first_inside_orbit = 0; - if ((_mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || - _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || - _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || - _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + /* TODO: extend this to different frames, global for now */ - _mission_item_triplet.previous_valid = true; - memcpy(&_mission_item_triplet.previous, &_mission_item[previous_setpoint_index], sizeof(mission_item_s)); - warnx("previous is valid"); - break; + _mission_item_triplet.current_valid = false; + + if (_mission_item_count > 0 && new_mission_item_index < _mission_item_count) { + _mission_item_triplet.current_valid = true; + memcpy(&_mission_item_triplet.current, &_mission_item[new_mission_item_index], sizeof(mission_item_s)); } - previous_setpoint_index--; - } + int previous_setpoint_index = -1; + _mission_item_triplet.previous_valid = false; - /* - * Check if next WP (in mission, not in execution order) - * is available and identify correct index - */ - int next_setpoint_index = -1; - _mission_item_triplet.next_valid = false; + if (new_mission_item_index > 0) { + previous_setpoint_index = new_mission_item_index - 1; + } - /* next waypoint */ - if (_mission_item_count > 1) { - next_setpoint_index = new_mission_item_index + 1; - } + while (previous_setpoint_index >= 0) { - while (next_setpoint_index < _mission_item_count - 1) { + if ((_mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || + _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || + _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || + _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { - if ((_mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || - _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || - _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || - _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + _mission_item_triplet.previous_valid = true; + memcpy(&_mission_item_triplet.previous, &_mission_item[previous_setpoint_index], sizeof(mission_item_s)); + break; + } - _mission_item_triplet.next_valid = true; - memcpy(&_mission_item_triplet.next, &_mission_item[next_setpoint_index], sizeof(mission_item_s)); - warnx("next is valid"); - break; + previous_setpoint_index--; } - next_setpoint_index++; - } + /* + * Check if next WP (in mission, not in execution order) + * is available and identify correct index + */ + int next_setpoint_index = -1; + _mission_item_triplet.next_valid = false; + + /* next waypoint */ + if (_mission_item_count > 1) { + next_setpoint_index = new_mission_item_index + 1; + } + while (next_setpoint_index < _mission_item_count - 1) { + + if ((_mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || + _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || + _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || + _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + + _mission_item_triplet.next_valid = true; + memcpy(&_mission_item_triplet.next, &_mission_item[next_setpoint_index], sizeof(mission_item_s)); + break; + } + + next_setpoint_index++; + } + } /* lazily publish the setpoint only once available */ if (_triplet_pub > 0) { @@ -789,7 +820,6 @@ Navigator::change_current_mission_item(int new_mission_item_index) /* advertise and publish */ _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); } - } |