diff options
author | Julian Oes <julian@oes.ch> | 2013-11-20 17:05:17 +0100 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-11-20 17:05:17 +0100 |
commit | 7c741073fedf878af6a65100f00cd554fd5e332c (patch) | |
tree | e122b8206fe3c472853a23f0f1b50bb093dd5e6b | |
parent | c33d61693519ee48f50e49fa086602f52eab47ec (diff) | |
download | px4-firmware-7c741073fedf878af6a65100f00cd554fd5e332c.tar.gz px4-firmware-7c741073fedf878af6a65100f00cd554fd5e332c.tar.bz2 px4-firmware-7c741073fedf878af6a65100f00cd554fd5e332c.zip |
Navigator: Only support 10WPs for now
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 69 |
1 files changed, 35 insertions, 34 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a255797c6..075e1a26f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -132,10 +132,10 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - unsigned _mission_item_count; /**< maximum number of mission items supported */ - struct mission_item_s * _mission_items; /**< storage for mission */ - bool _mission_valid; /**< flag if mission is valid */ - + unsigned _max_mission_item_count; /**< maximum number of mission items supported */ + + unsigned _mission_item_count; /** number of mission items copied */ + struct mission_item_s *_mission_item; /**< storage for mission */ struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ @@ -236,8 +236,7 @@ Navigator::Navigator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ - _mission_item_count(0), - _mission_valid(false), + _max_mission_item_count(10), _fence_valid(false), _inside_fence(true), _loiter_hold(false) @@ -246,6 +245,8 @@ Navigator::Navigator() : memset(&_fence, 0, sizeof(_fence)); _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); + _mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count); + /* fetch initial values */ parameters_update(); } @@ -292,28 +293,25 @@ Navigator::mission_update() // XXX this is not optimal yet, but a first prototype / // test implementation - 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"); - } - } + if (mission.count <= _max_mission_item_count) { + /* + * Perform an atomic copy & state update + */ + irqstate_t flags = irqsave(); - /* - * Perform an atomic copy & state update - */ - irqstate_t flags = irqsave(); + memcpy(_mission_item, mission.items, mission.count * sizeof(struct mission_item_s)); + _mission_item_count = mission.count; - memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s)); - _mission_valid = true; - _mission_item_count = mission.count; + irqrestore(flags); - irqrestore(flags); + current_waypoint_changed(0); + } else { + warnx("ERROR: too many waypoints, not supported"); + } /* Reset to 0 for now when a waypoint is changed */ /* TODO add checks if and how the mission has changed */ - current_waypoint_changed(0); + } } @@ -447,7 +445,7 @@ Navigator::task_main() if (1 /* autonomous flight */) { /* execute navigation once we have a setpoint */ - if (_mission_valid) { + if (_mission_item_count > 0) { // Next waypoint math::Vector2f prev_wp; @@ -690,7 +688,8 @@ Navigator::current_waypoint_changed(unsigned new_setpoint_index) 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)); + memcpy(&_mission_item_triplet.current, &_mission_item[new_setpoint_index], sizeof(mission_item_s)); + warnx("current is valid"); } int previous_setpoint_index = -1; @@ -702,13 +701,14 @@ Navigator::current_waypoint_changed(unsigned new_setpoint_index) 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)) { + 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)) { _mission_item_triplet.previous_valid = true; - memcpy(&_mission_item_triplet.previous, &_mission_items[previous_setpoint_index], sizeof(mission_item_s)); + memcpy(&_mission_item_triplet.previous, &_mission_item[previous_setpoint_index], sizeof(mission_item_s)); + warnx("previous is valid"); break; } @@ -729,13 +729,14 @@ Navigator::current_waypoint_changed(unsigned new_setpoint_index) 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)) { + 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_items[next_setpoint_index], sizeof(mission_item_s)); + memcpy(&_mission_item_triplet.next, &_mission_item[next_setpoint_index], sizeof(mission_item_s)); + warnx("next is valid"); break; } |