diff options
author | Julian Oes <julian@oes.ch> | 2013-11-20 12:53:05 +0100 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-11-20 12:53:05 +0100 |
commit | a27c7e831945f0a6b95b50b9ac68364b28a49362 (patch) | |
tree | 1e8160a28fd75fa892200512fc038c439d989ce1 /src/modules/navigator | |
parent | a6c5a1920639f3ff1b2d1a0cea6eeacf6676fc76 (diff) | |
download | px4-firmware-a27c7e831945f0a6b95b50b9ac68364b28a49362.tar.gz px4-firmware-a27c7e831945f0a6b95b50b9ac68364b28a49362.tar.bz2 px4-firmware-a27c7e831945f0a6b95b50b9ac68364b28a49362.zip |
Navigator: Added simple mission triplet publication on waypoint change
Diffstat (limited to 'src/modules/navigator')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 109 |
1 files changed, 97 insertions, 12 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5d4a6ef94..a255797c6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -201,6 +201,8 @@ private: void publish_safepoints(unsigned points); bool fence_valid(const struct fence_s &fence); + + void current_waypoint_changed(unsigned next_setpoint_index); }; namespace navigator @@ -234,7 +236,7 @@ Navigator::Navigator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ - _mission_items_maxcount(20), + _mission_item_count(0), _mission_valid(false), _fence_valid(false), _inside_fence(true), @@ -290,19 +292,28 @@ Navigator::mission_update() // XXX this is not optimal yet, but a first prototype / // test implementation - if (mission.count <= _mission_items_maxcount) { - /* - * Perform an atomic copy & state update - */ - irqstate_t flags = irqsave(); + if (mission.count > _mission_item_count) { + _mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_item_count); + if (!_mission_items) { + _mission_item_count = 0; + warnx("no free RAM to allocate mission, rejecting any waypoints"); + } + } - memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s)); - _mission_valid = true; + /* + * Perform an atomic copy & state update + */ + irqstate_t flags = irqsave(); - irqrestore(flags); - } else { - warnx("mission larger than storage space"); - } + memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s)); + _mission_valid = true; + _mission_item_count = mission.count; + + irqrestore(flags); + + /* Reset to 0 for now when a waypoint is changed */ + /* TODO add checks if and how the mission has changed */ + current_waypoint_changed(0); } } @@ -670,6 +681,80 @@ Navigator::fence_point(int argc, char *argv[]) errx(1, "can't store fence point"); } +void +Navigator::current_waypoint_changed(unsigned new_setpoint_index) +{ + /* TODO: extend this to different frames, global for now */ + + _mission_item_triplet.current_valid = false; + + if (_mission_item_count > 0 && new_setpoint_index < _mission_item_count) { + _mission_item_triplet.current_valid = true; + memcpy(&_mission_item_triplet.current, &_mission_items[new_setpoint_index], sizeof(mission_item_s)); + } + + int previous_setpoint_index = -1; + _mission_item_triplet.previous_valid = false; + + if (new_setpoint_index > 0) { + previous_setpoint_index = new_setpoint_index - 1; + } + + while (previous_setpoint_index >= 0) { + + if ((_mission_items[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || + _mission_items[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || + _mission_items[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || + _mission_items[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + + _mission_item_triplet.previous_valid = true; + memcpy(&_mission_item_triplet.previous, &_mission_items[previous_setpoint_index], sizeof(mission_item_s)); + break; + } + + previous_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_setpoint_index + 1; + } + + while (next_setpoint_index < _mission_item_count - 1) { + + if ((_mission_items[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || + _mission_items[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || + _mission_items[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || + _mission_items[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + + _mission_item_triplet.next_valid = true; + memcpy(&_mission_item_triplet.next, &_mission_items[next_setpoint_index], sizeof(mission_item_s)); + break; + } + + next_setpoint_index++; + } + + + /* lazily publish the setpoint only once available */ + if (_triplet_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); + + } else { + /* advertise and publish */ + _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); + } + +} + static void usage() { errx(1, "usage: navigator {start|stop|status|fence}"); |