diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index a4e31bda65..a9ee26eacf 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -60,6 +60,10 @@ bool debug = false; bool verbose = false; +orb_advert_t mission_pub = -1; +struct mission_s mission; + +#define NUM_MISSIONS_SUPPORTED 10 //#define MAVLINK_WPM_NO_PRINTF #define MAVLINK_WPM_VERBOSE 1 @@ -78,9 +82,10 @@ void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavl mission_item->radius = mavlink_mission_item->param1; mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */ mission_item->autocontinue = mavlink_mission_item->autocontinue; + mission_item->index = mavlink_mission_item->seq; } -void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, const uint16_t seq, mavlink_mission_item_t *mavlink_mission_item) +void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) { mavlink_mission_item->x = (float)mission_item->lat; @@ -92,8 +97,7 @@ void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missi mavlink_mission_item->param1 = mission_item->radius; mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ mavlink_mission_item->autocontinue = mission_item->autocontinue; - - mavlink_mission_item->seq = seq; + mavlink_mission_item->seq = mission_item->index; } void mavlink_wpm_init(mavlink_wpm_storage *state) @@ -117,6 +121,13 @@ void mavlink_wpm_init(mavlink_wpm_storage *state) state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value + mission.count = 0; + mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * NUM_MISSIONS_SUPPORTED); + if (!mission.items) { + mission.count = 0; + /* XXX reject waypoints if this fails */ + warnx("no free RAM to allocate mission, rejecting any waypoints"); + } } /* @@ -558,9 +569,6 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos) { - static orb_advert_t mission_pub = -1; - static struct mission_s mission; - uint64_t now = mavlink_missionlib_get_system_timestamp(); switch (msg->msgid) { @@ -711,7 +719,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->current_wp_id = wpr.seq; mavlink_mission_item_t wp; - map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], wpr.seq, &wp); + map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], &wp); mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp); } else { @@ -835,14 +843,11 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->current_partner_compid = msg->compid; wpm->current_count = wpc.count; - /* prepare mission topic */ - mission.count = wpc.count; - mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * mission.count); - - if (!mission.items) { - mission.count = 0; - /* XXX reject waypoints if this fails */ - warnx("no free RAM to allocate mission, rejecting any waypoints"); + if (wpc.count > NUM_MISSIONS_SUPPORTED) { + warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); + } else { + /* prepare mission topic */ + mission.count = wpc.count; } #ifdef MAVLINK_WPM_NO_PRINTF @@ -1087,8 +1092,26 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->yaw_reached = false; wpm->pos_reached = false; + /* prepare mission topic */ + mission.count = 0; + memset(mission.items, 0, sizeof(struct mission_item_s)*NUM_MISSIONS_SUPPORTED); + + /* Initialize mission publication if necessary */ + if (mission_pub < 0) { + mission_pub = orb_advertise(ORB_ID(mission), &mission); + + } else { + orb_publish(ORB_ID(mission), mission_pub, &mission); + } + + + warnx("Mission cleared"); + + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state); + warnx("not cleared"); } break; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fc2f6d3801..9066a3b424 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -69,11 +69,6 @@ #include #include -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; /** * navigator app start / stop handling function @@ -142,8 +137,6 @@ private: unsigned _mission_item_count; /** number of mission items copied */ struct mission_item_s *_mission_item; /**< storage for mission */ - int _current_mission_item_index; /** current active mission item , -1 for none */ - struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ @@ -213,9 +206,11 @@ private: bool fence_valid(const struct fence_s &fence); - int add_mission_item(unsigned mission_item, struct mission_item_s *new_mission_item); + void add_mission_item(unsigned mission_item_index, + const struct mission_item_s *existing_mission_item, + struct mission_item_s *new_mission_item); - void add_last_mission_item(const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item); + void update_mission_item_triplet(); void advance_current_mission_item(); @@ -260,7 +255,6 @@ Navigator::Navigator() : _max_mission_item_count(10), _fence_valid(false), _inside_fence(true), - _current_mission_item_index(-1), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), @@ -626,27 +620,20 @@ Navigator::fence_point(int argc, char *argv[]) errx(1, "can't store fence point"); } -int -Navigator::add_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) { +void +Navigator::add_mission_item(unsigned mission_item_index, const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item) { /* Check if there is a further mission as the new next item */ while (mission_item_index < _mission_item_count) { if (1 /* TODO: check for correct frame */) { - warnx("copying item number %d", mission_item_index); memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s)); - return OK; + return; } mission_item_index++; } - return ERROR; -} - -void -Navigator::add_last_mission_item(const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item) -{ /* if no existing mission item exists, take curent location */ if (existing_mission_item == nullptr) { @@ -706,54 +693,71 @@ Navigator::add_last_mission_item(const struct mission_item_s *existing_mission_i } void -Navigator::advance_current_mission_item() +Navigator::update_mission_item_triplet() { - /* if there is one more mission available we can just advance by one, otherwise return */ - if (_mission_item_triplet.next_valid) { - - reset_mission_item_reached(); - - /* copy current mission to previous item */ - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - - /* copy the next to current */ - memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s)); - _mission_item_triplet.current_valid = _mission_item_triplet.next_valid; - - _current_mission_item_index++; - + if (!_mission_item_triplet.current_valid) { - /* maybe there are no more mission item, in this case add a loiter mission item */ - if (add_mission_item(_current_mission_item_index + 1, &_mission_item_triplet.next) != OK) { + /* the current mission item is missing, add one */ + if (_mission_item_triplet.previous_valid) { + /* if we know the last one, proceed to succeeding one */ + add_mission_item(_mission_item_triplet.previous.index + 1, &_mission_item_triplet.previous, &_mission_item_triplet.current); + } + else { + /* if we don't remember the last one, start new */ + add_mission_item(0, nullptr, &_mission_item_triplet.current); + } + _mission_item_triplet.current_valid = true; + } - add_last_mission_item(&_mission_item_triplet.current, &_mission_item_triplet.next); + if (_mission_item_triplet.current_valid && !_mission_item_triplet.next_valid) { + + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + /* if we are already loitering, don't bother about a next mission item */ + + _mission_item_triplet.next_valid = false; + } else { + + add_mission_item(_mission_item_triplet.current.index + 1, &_mission_item_triplet.current, &_mission_item_triplet.next); _mission_item_triplet.next_valid = true; } } } +void +Navigator::advance_current_mission_item() +{ + /* if there is no more mission available, don't advance and return */ + if (!_mission_item_triplet.next_valid) { + return; + } + + reset_mission_item_reached(); + + /* copy current mission to previous item */ + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + + /* copy the next to current */ + memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s)); + _mission_item_triplet.current_valid = _mission_item_triplet.next_valid; + + /* flag the next mission as invalid */ + _mission_item_triplet.next_valid = false; + + update_mission_item_triplet(); +} + void Navigator::restart_mission() { reset_mission_item_reached(); - _current_mission_item_index = 0; - + /* forget about the all mission items */ _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = false; + _mission_item_triplet.next_valid = false; - /* add a new current mission item */ - if (add_mission_item(_current_mission_item_index, &_mission_item_triplet.current) != OK) { - - add_last_mission_item(nullptr, &_mission_item_triplet.current); - } else { - /* if current succeeds, we can even add a next item */ - if (add_mission_item(_current_mission_item_index + 1, &_mission_item_triplet.next) != OK) { - add_last_mission_item(&_mission_item_triplet.current, &_mission_item_triplet.next); - } - _mission_item_triplet.next_valid = true; - } - _mission_item_triplet.current_valid = true; + update_mission_item_triplet(); } diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index f97de94bc4..a39a1e4d7e 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -80,6 +80,7 @@ struct mission_item_s float radius; /**< radius in which the mission is accepted as reached in meters */ float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ bool autocontinue; /**< true if next waypoint should follow after this one */ + int index; /**< index matching the mavlink waypoint */ }; struct mission_s diff --git a/src/modules/uORB/topics/mission_item_triplet.h b/src/modules/uORB/topics/mission_item_triplet.h index 48553b0ac5..b35eae6070 100644 --- a/src/modules/uORB/topics/mission_item_triplet.h +++ b/src/modules/uORB/topics/mission_item_triplet.h @@ -67,6 +67,10 @@ struct mission_item_triplet_s struct mission_item_s previous; struct mission_item_s current; struct mission_item_s next; + + int previous_index; + int current_index; + int next_index; }; /**