diff options
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 102 |
1 files changed, 98 insertions, 4 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b88fc804c..1c81245e0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -141,6 +141,7 @@ private: int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _mission_sub; /**< notification of mission updates */ + int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ @@ -156,9 +157,12 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ unsigned _max_mission_item_count; /**< maximum number of mission items supported */ + unsigned _max_onboard_mission_item_count;/**< maximum number of onboard mission items supported */ unsigned _mission_item_count; /** number of mission items copied */ + unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ struct mission_item_s *_mission_item; /**< storage for mission */ + struct mission_item_s *_onboard_mission_item; /**< storage for onboard mission */ struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ @@ -170,18 +174,22 @@ private: bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; bool _mission_item_reached; + bool _onboard_mission_item_reached; navigation_mode_t _mode; unsigned _current_mission_index; + unsigned _current_onboard_mission_index; struct { float min_altitude; float loiter_radius; + int onboard_mission_enabled; } _parameters; /**< local copies of parameters */ struct { param_t min_altitude; param_t loiter_radius; + param_t onboard_mission_enabled; } _parameter_handles; /**< handles for parameters */ @@ -197,6 +205,11 @@ private: void mission_update(); /** + * Retrieve onboard mission. + */ + void onboard_mission_update(); + + /** * Shim for calling task_main from task_create. */ static void task_main_trampoline(int argc, char *argv[]); @@ -268,6 +281,7 @@ Navigator::Navigator() : _vstatus_sub(-1), _params_sub(-1), _mission_sub(-1), + _onboard_mission_sub(-1), _capabilities_sub(-1), /* publications */ @@ -279,22 +293,28 @@ Navigator::Navigator() : _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ _max_mission_item_count(10), + _max_onboard_mission_item_count(10), _mission_item_count(0), + _onboard_mission_item_count(0), _fence_valid(false), _inside_fence(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _mission_item_reached(false), + _onboard_mission_item_reached(false), _mode(NAVIGATION_MODE_NONE), - _current_mission_index(0) + _current_mission_index(0), + _current_onboard_mission_index(0) { _global_pos.valid = false; memset(&_fence, 0, sizeof(_fence)); _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); + _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); _mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count); + _onboard_mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_onboard_mission_item_count); _mission_item_triplet.previous_valid = false; _mission_item_triplet.current_valid = false; @@ -340,6 +360,7 @@ Navigator::parameters_update() param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); + param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); return OK; } @@ -410,6 +431,71 @@ Navigator::mission_update() } void +Navigator::onboard_mission_update() +{ + struct mission_s onboard_mission; + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { + // XXX this is not optimal yet, but a first prototype / + // test implementation + + if (onboard_mission.count <= _max_onboard_mission_item_count) { + + /* Check if first part of mission (up to _current_onboard_mission_index - 1) changed: + * if the first part changed: start again at first waypoint + * if the first part remained unchanged: continue with the (possibly changed second part) + */ + if (onboard_mission.current_index == -1 && _current_onboard_mission_index < _onboard_mission_item_count && _current_onboard_mission_index < onboard_mission.count) { //check if not finished and if the new mission is not a shorter mission + for (unsigned i = 0; i < _current_onboard_mission_index; i++) { + if (!cmp_mission_item_equivalent(_onboard_mission_item[i], onboard_mission.items[i])) { + /* set flag to restart mission next we're in auto */ + _current_onboard_mission_index = 0; + mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index); + //warnx("First part of onboard mission differs i=%d", i); + break; + } +// else { +// warnx("Onboard mission item is equivalent i=%d", i); +// } + } + } else if (onboard_mission.current_index >= 0 && onboard_mission.current_index < onboard_mission.count) { + /* set flag to restart mission next we're in auto */ + _current_onboard_mission_index = onboard_mission.current_index; + mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index); + } else { + _current_onboard_mission_index = 0; + mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index); + } + + /* + * Perform an atomic copy & state update + */ + irqstate_t flags = irqsave(); + + memcpy(_onboard_mission_item, onboard_mission.items, onboard_mission.count * sizeof(struct mission_item_s)); + _onboard_mission_item_count = onboard_mission.count; + + irqrestore(flags); + + + + } else { + warnx("ERROR: too many waypoints, not supported"); + mavlink_log_critical(_mavlink_fd, "[navigator] too many waypoints, not supported"); + _onboard_mission_item_count = 0; + } + + if (_mode == NAVIGATION_MODE_WAYPOINT) { + start_waypoint(); + } + + /* TODO add checks if and how the mission has changed */ + } else { + _onboard_mission_item_count = 0; + _current_onboard_mission_index = 0; + } +} + +void Navigator::task_main_trampoline(int argc, char *argv[]) { navigator::g_navigator->task_main(); @@ -428,6 +514,7 @@ Navigator::task_main() */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _mission_sub = orb_subscribe(ORB_ID(mission)); + _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -439,6 +526,7 @@ Navigator::task_main() } mission_update(); + onboard_mission_update(); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -453,7 +541,7 @@ Navigator::task_main() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); /* wakeup source(s) */ - struct pollfd fds[6]; + struct pollfd fds[7]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -466,8 +554,10 @@ Navigator::task_main() fds[3].events = POLLIN; fds[4].fd = _mission_sub; fds[4].events = POLLIN; - fds[5].fd = _vstatus_sub; + fds[5].fd = _onboard_mission_sub; fds[5].events = POLLIN; + fds[6].fd = _vstatus_sub; + fds[6].events = POLLIN; while (!_task_should_exit) { @@ -489,7 +579,7 @@ Navigator::task_main() perf_begin(_loop_perf); /* only update vehicle status if it changed */ - if (fds[5].revents & POLLIN) { + if (fds[6].revents & POLLIN) { /* read from param to clear updated flag */ orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); @@ -570,6 +660,10 @@ Navigator::task_main() mission_update(); } + if (fds[5].revents & POLLIN) { + onboard_mission_update(); + } + if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); } |