From 83b09614e71c7ea68db1081a673e53bca2d9422f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Dec 2013 16:25:11 +0100 Subject: [PATCH 1/7] Dataman: Start dataman and use it in waypoints and navigator instead of mission items in mission topic --- ROMFS/px4fmu_common/init.d/rcS | 5 + src/modules/mavlink/waypoints.c | 108 ++++++----- src/modules/navigator/navigator_main.cpp | 233 ++++++----------------- src/modules/uORB/topics/mission.h | 10 +- 4 files changed, 129 insertions(+), 227 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5f52969d13..9445e963be 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -111,6 +111,11 @@ then # commander start + # + # Start the datamanager + # + dataman start + # # Start the Navigator # diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 809900d7d4..7aad5038d2 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -52,10 +52,8 @@ #include #include - -#ifndef FM_PI -#define FM_PI 3.1415926535897932384626433832795f -#endif +#include +#include bool debug = false; bool verbose = false; @@ -63,13 +61,22 @@ 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 uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; +void publish_mission() +{ + /* 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); + } +} + int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) { /* only support global waypoints for now */ @@ -96,7 +103,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli return MAV_MISSION_ERROR; } - mission_item->yaw = mavlink_mission_item->param4*M_DEG_TO_RAD_F; + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F); mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = mavlink_mission_item->command; @@ -104,6 +111,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli 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; + mission_item->origin = ORIGIN_MAVLINK; return OK; } @@ -151,14 +159,6 @@ void mavlink_wpm_init(mavlink_wpm_storage *state) // state->pos_reached = false; ///< boolean for position reached // 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"); - } } /* @@ -597,13 +597,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi mission.current_index = wpc.seq; - /* 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); - } + publish_mission(); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); @@ -703,16 +697,24 @@ 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], &wp); - if (mission.current_index == wpr.seq) { - wp.current = true; + struct mission_item_s mission_item; + ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(DM_KEY_WAYPOINTS, wpr.seq, &mission_item, len) == len) { + + if (mission.current_index == wpr.seq) { + wp.current = true; + } else { + wp.current = false; + } + + map_mission_item_to_mavlink_mission_item(&mission_item, &wp); + mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp); } else { - wp.current = false; + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); } - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp); - } else { // if (verbose) { @@ -815,7 +817,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); -#endif +#endif } if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { @@ -837,10 +839,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi 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; - /* reset current index */ - mission.current_index = -1; + /* set count to 0 while copying */ + mission.count = 0; + publish_mission(); } #ifdef MAVLINK_WPM_NO_PRINTF @@ -962,20 +963,26 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]); // memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission.items[wp.seq]); + struct mission_item_s mission_item; + + int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); + if (ret != OK) { - - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret); wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } + size_t len = sizeof(struct mission_item_s); + + if (dm_write(DM_KEY_WAYPOINTS, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + break; + } + if (wp.current) { mission.current_index = wp.seq; - warnx("current is: %d", wp.seq); - } else { - warnx("not current"); } wpm->current_wp_id = wp.seq + 1; @@ -1009,14 +1016,10 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // } // TODO: update count? - /* 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); - } + mission.count = wpm->current_count; + + publish_mission(); wpm->size = wpm->current_count; @@ -1113,20 +1116,15 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi /* 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); + if (dm_clear(DM_KEY_WAYPOINTS) == OK) { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); } else { - orb_publish(ORB_ID(mission), mission_pub, &mission); + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); } + publish_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); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1c81245e0c..c6aac6af11 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -141,7 +142,6 @@ 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 */ @@ -157,12 +157,8 @@ 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 */ @@ -174,11 +170,9 @@ 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; @@ -204,11 +198,6 @@ private: */ void mission_update(); - /** - * Retrieve onboard mission. - */ - void onboard_mission_update(); - /** * Shim for calling task_main from task_create. */ @@ -281,7 +270,6 @@ Navigator::Navigator() : _vstatus_sub(-1), _params_sub(-1), _mission_sub(-1), - _onboard_mission_sub(-1), _capabilities_sub(-1), /* publications */ @@ -293,19 +281,15 @@ 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_onboard_mission_index(0) + _current_mission_index(0) { _global_pos.valid = false; memset(&_fence, 0, sizeof(_fence)); @@ -313,9 +297,6 @@ Navigator::Navigator() : _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; _mission_item_triplet.next_valid = false; @@ -370,131 +351,50 @@ Navigator::mission_update() { struct mission_s mission; if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { - // XXX this is not optimal yet, but a first prototype / - // test implementation - if (mission.count <= _max_mission_item_count) { - - /* Check if first part of mission (up to _current_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 (mission.current_index == -1 && _current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission - for (unsigned i = 0; i < _current_mission_index; i++) { - if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { - /* set flag to restart mission next we're in auto */ - _current_mission_index = 0; - mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); - //warnx("First part of mission differs i=%d", i); - break; - } -// else { -// warnx("Mission item is equivalent i=%d", i); -// } - } - } else if (mission.current_index >= 0 && mission.current_index < mission.count) { - /* set flag to restart mission next we're in auto */ - _current_mission_index = mission.current_index; - mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); - } else { - _current_mission_index = 0; - mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); - } +// /* Check if first part of mission (up to _current_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 (mission.current_index == -1 && _current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission +// for (unsigned i = 0; i < _current_mission_index; i++) { +// if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { +// /* set flag to restart mission next we're in auto */ +// _current_mission_index = 0; +// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); +// //warnx("First part of mission differs i=%d", i); +// break; +// } +// // else { +// // warnx("Mission item is equivalent i=%d", i); +// // } +// } +// } else if (mission.current_index >= 0 && mission.current_index < mission.count) { +// /* set flag to restart mission next we're in auto */ +// _current_mission_index = mission.current_index; +// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); +// } else { +// _current_mission_index = 0; +// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); +// } - /* - * Perform an atomic copy & state update - */ - irqstate_t flags = irqsave(); + _mission_item_count = mission.count; + _current_mission_index = mission.current_index; - memcpy(_mission_item, mission.items, mission.count * sizeof(struct mission_item_s)); - _mission_item_count = mission.count; - - irqrestore(flags); - - - - } else { - warnx("ERROR: too many waypoints, not supported"); - mavlink_log_critical(_mavlink_fd, "[navigator] too many waypoints, not supported"); - _mission_item_count = 0; - } - - if (_mode == NAVIGATION_MODE_WAYPOINT) { - start_waypoint(); - } - - /* TODO add checks if and how the mission has changed */ } else { _mission_item_count = 0; _current_mission_index = 0; } -} - -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; + if (_mission_item_count == 0 && _mode == NAVIGATION_MODE_WAYPOINT) { + set_mode(NAVIGATION_MODE_LOITER); + } + else if (_mode == NAVIGATION_MODE_WAYPOINT) { + start_waypoint(); } } + + void Navigator::task_main_trampoline(int argc, char *argv[]) { @@ -514,7 +414,6 @@ 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)); @@ -526,7 +425,6 @@ Navigator::task_main() } mission_update(); - onboard_mission_update(); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -541,7 +439,7 @@ Navigator::task_main() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[6]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -554,10 +452,8 @@ Navigator::task_main() fds[3].events = POLLIN; fds[4].fd = _mission_sub; fds[4].events = POLLIN; - fds[5].fd = _onboard_mission_sub; + fds[5].fd = _vstatus_sub; fds[5].events = POLLIN; - fds[6].fd = _vstatus_sub; - fds[6].events = POLLIN; while (!_task_should_exit) { @@ -579,7 +475,7 @@ Navigator::task_main() perf_begin(_loop_perf); /* only update vehicle status if it changed */ - if (fds[6].revents & POLLIN) { + if (fds[5].revents & POLLIN) { /* read from param to clear updated flag */ orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); @@ -660,10 +556,6 @@ 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); } @@ -974,22 +866,28 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) int Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) { - if (mission_item_index < _mission_item_count) { - memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s)); - - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = (double)_home_pos.lat / 1e7; - new_mission_item->lon = (double)_home_pos.lon / 1e7; - new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; - new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_mission_item->radius = 50.0f; // TODO: get rid of magic number - } - // warnx("added mission item: %d", mission_item_index); - return OK; + if (mission_item_index >= _mission_item_count) { + return ERROR; } - // warnx("could not add mission item: %d", mission_item_index); - return ERROR; + + struct mission_item_s mission_item; + + if (dm_read(DM_KEY_WAYPOINTS, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + return ERROR; + } + + memcpy(new_mission_item, &mission_item, sizeof(struct mission_item_s)); + + if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* if it is a RTL waypoint, append the home position */ + new_mission_item->lat = (double)_home_pos.lat / 1e7; + new_mission_item->lon = (double)_home_pos.lon / 1e7; + new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_mission_item->radius = 50.0f; // TODO: get rid of magic number + } + + return OK; } void @@ -1036,8 +934,6 @@ Navigator::advance_current_mission_item() return ERROR; } - 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; @@ -1182,14 +1078,11 @@ Navigator::start_waypoint() { reset_mission_item_reached(); - /* this means we should start fresh */ - if (_current_mission_index == 0) { - - _mission_item_triplet.previous_valid = false; - - } else { + if (_current_mission_index > 0) { set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous); _mission_item_triplet.previous_valid = true; + } else { + _mission_item_triplet.previous_valid = false; } set_waypoint_mission_item(_current_mission_index, &_mission_item_triplet.current); diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 2427a1d579..30f06c3597 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -46,6 +46,8 @@ #include #include "../uORB.h" +#define NUM_MISSIONS_SUPPORTED 256 + /* compatible to mavlink MAV_CMD */ enum NAV_CMD { NAV_CMD_WAYPOINT=16, @@ -59,6 +61,11 @@ enum NAV_CMD { NAV_CMD_PATHPLANNING=81 }; +enum ORIGIN { + ORIGIN_MAVLINK = 0, + ORIGIN_ONBOARD +}; + /** * @addtogroup topics * @{ @@ -84,11 +91,11 @@ struct mission_item_s 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 */ + enum ORIGIN origin; /**< where the waypoint has been generated */ }; struct mission_s { - struct mission_item_s *items; unsigned count; int current_index; /**< default -1, start at the one changed latest */ }; @@ -99,6 +106,5 @@ struct mission_s /* register this as object request broker structure */ ORB_DECLARE(mission); -ORB_DECLARE(onboard_mission); #endif From e034f5135ebabeb751ea775f2d79440cf74c8047 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Dec 2013 16:25:46 +0100 Subject: [PATCH 2/7] Dataman: Some minor fixes --- src/modules/dataman/dataman.c | 16 +++++++--------- src/modules/dataman/dataman.h | 5 ++--- src/systemcmds/tests/test_dataman.c | 23 +++++++++++++---------- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 05b77da20d..dd3573d9a3 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -33,10 +33,8 @@ * ****************************************************************************/ /** - * @file navigator_main.c - * Implementation of the main navigation state machine. - * - * Handles missions, geo fencing and failsafe navigation behavior. + * @file dataman.c + * DATAMANAGER driver. */ #include @@ -113,7 +111,7 @@ static unsigned g_func_counts[dm_number_of_funcs]; static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_SAFE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX, - DM_KEY_WAY_POINTS_MAX, + DM_KEY_WAYPOINTS_MAX, }; /* Table of offset for index 0 of each item type */ @@ -138,7 +136,7 @@ static work_q_t g_work_q; sem_t g_work_queued_sema; sem_t g_init_sema; -static bool g_task_should_exit; /**< if true, sensor task should exit */ +static bool g_task_should_exit; /**< if true, dataman task should exit */ #define DM_SECTOR_HDR_SIZE 4 static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; @@ -266,11 +264,11 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v /* Get the offset for this item */ offset = calculate_offset(item, index); - if (offset < 0) + if (offset < 0) return -1; /* Make sure caller has not given us more data than we can handle */ - if (count > DM_MAX_DATA_SIZE) + if (count > DM_MAX_DATA_SIZE) return -1; /* Write out the data, prefixed with length and persistence level */ @@ -456,7 +454,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const return -1; /* Will return with queues locked */ - if ((work = create_work_item()) == NULL) + if ((work = create_work_item()) == NULL) return -1; /* queues unlocked on failure */ work->func = dm_write_func; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 41ddfaf61b..9e1f789ad3 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -41,7 +41,6 @@ #include #include -#include #ifdef __cplusplus extern "C" { @@ -51,7 +50,7 @@ extern "C" { typedef enum { DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAY_POINTS, /* Mission way point coordinates */ + DM_KEY_WAYPOINTS, /* Mission way point coordinates */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -59,7 +58,7 @@ extern "C" { enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - DM_KEY_WAY_POINTS_MAX = MAVLINK_WPM_MAX_WP_COUNT, + DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED }; /* Data persistence levels */ diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index e33c5acebe..7de87b476f 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -83,12 +83,15 @@ task_main(int argc, char *argv[]) srand(hrt_absolute_time() ^ my_id); unsigned hit = 0, miss = 0; wstart = hrt_absolute_time(); - for (unsigned i = 0; i < 256; i++) { + for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { memset(buffer, my_id, sizeof(buffer)); buffer[1] = i; unsigned hash = i ^ my_id; unsigned len = (hash & 63) + 2; - if (dm_write(DM_KEY_WAY_POINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len) != len) { + + int ret = dm_write(DM_KEY_WAYPOINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); + warnx("ret: %d", ret); + if (ret != len) { warnx("%d write failed, index %d, length %d", my_id, hash, len); goto fail; } @@ -97,10 +100,10 @@ task_main(int argc, char *argv[]) rstart = hrt_absolute_time(); wend = rstart; - for (unsigned i = 0; i < 256; i++) { + for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { unsigned hash = i ^ my_id; unsigned len2, len = (hash & 63) + 2; - if ((len2 = dm_read(DM_KEY_WAY_POINTS, hash, buffer, sizeof(buffer))) < 2) { + if ((len2 = dm_read(DM_KEY_WAYPOINTS, hash, buffer, sizeof(buffer))) < 2) { warnx("%d read failed length test, index %d", my_id, hash); goto fail; } @@ -120,7 +123,7 @@ task_main(int argc, char *argv[]) } rend = hrt_absolute_time(); warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.", - my_id, hit, miss, (rend - rstart) / 256000, (wend - wstart) / 256000); + my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000); sem_post(sems + my_id); return 0; fail: @@ -159,18 +162,18 @@ int test_dataman(int argc, char *argv[]) } free(sems); dm_restart(DM_INIT_REASON_IN_FLIGHT); - for (i = 0; i < 256; i++) { - if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) + for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { + if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) break; } - if (i >= 256) { + if (i >= NUM_MISSIONS_SUPPORTED) { warnx("Restart in-flight failed"); return -1; } dm_restart(DM_INIT_REASON_POWER_ON); - for (i = 0; i < 256; i++) { - if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) { + for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { + if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) { warnx("Restart power-on failed"); return -1; } From e8df08f13905c2a71d71469ba7d6cecc23c2eb70 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Dec 2013 10:37:51 +0100 Subject: [PATCH 3/7] Dataman: Also reserve space for onboard missions --- src/modules/dataman/dataman.c | 1 + src/modules/dataman/dataman.h | 6 ++++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index dd3573d9a3..acd612d9e9 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -112,6 +112,7 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_SAFE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX, DM_KEY_WAYPOINTS_MAX, + DM_KEY_WAYPOINTS_ONBOARD_MAX }; /* Table of offset for index 0 of each item type */ diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 9e1f789ad3..dab96eb9b1 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -50,7 +50,8 @@ extern "C" { typedef enum { DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAYPOINTS, /* Mission way point coordinates */ + DM_KEY_WAYPOINTS, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -58,7 +59,8 @@ extern "C" { enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED + DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED }; /* Data persistence levels */ From bed40c962e94aaa9b1f398a201ef88096a35810a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Dec 2013 10:38:56 +0100 Subject: [PATCH 4/7] Navigator: handle onboard and mavlink missions --- src/modules/navigator/navigator_main.cpp | 174 +++++++++++++++-------- src/modules/uORB/topics/mission.h | 3 +- 2 files changed, 114 insertions(+), 63 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c6aac6af11..e2e2949e25 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -142,6 +142,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 */ @@ -155,11 +156,9 @@ private: struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ perf_counter_t _loop_perf; /**< loop performance counter */ - - unsigned _max_mission_item_count; /**< maximum number of mission items supported */ unsigned _mission_item_count; /** number of mission items copied */ - + unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ @@ -173,6 +172,7 @@ private: navigation_mode_t _mode; unsigned _current_mission_index; + unsigned _current_onboard_mission_index; struct { float min_altitude; @@ -198,6 +198,11 @@ private: */ void mission_update(); + /** + * Retrieve onboard mission. + */ + void onboard_mission_update(); + /** * Shim for calling task_main from task_create. */ @@ -216,7 +221,11 @@ private: void set_mode(navigation_mode_t new_nav_mode); - int set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item); + bool mission_possible(); + + bool onboard_mission_possible(); + + int set_waypoint_mission_item(dm_item_t dm_item, unsigned mission_item_index, struct mission_item_s *new_mission_item); void publish_mission_item_triplet(); @@ -270,6 +279,7 @@ Navigator::Navigator() : _vstatus_sub(-1), _params_sub(-1), _mission_sub(-1), + _onboard_mission_sub(-1), _capabilities_sub(-1), /* publications */ @@ -280,8 +290,8 @@ Navigator::Navigator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ - _max_mission_item_count(10), _mission_item_count(0), + _onboard_mission_item_count(0), _fence_valid(false), _inside_fence(true), _waypoint_position_reached(false), @@ -289,7 +299,8 @@ Navigator::Navigator() : _time_first_inside_orbit(0), _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)); @@ -352,32 +363,6 @@ Navigator::mission_update() struct mission_s mission; if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { -// /* Check if first part of mission (up to _current_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 (mission.current_index == -1 && _current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission -// for (unsigned i = 0; i < _current_mission_index; i++) { -// if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { -// /* set flag to restart mission next we're in auto */ -// _current_mission_index = 0; -// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); -// //warnx("First part of mission differs i=%d", i); -// break; -// } -// // else { -// // warnx("Mission item is equivalent i=%d", i); -// // } -// } -// } else if (mission.current_index >= 0 && mission.current_index < mission.count) { -// /* set flag to restart mission next we're in auto */ -// _current_mission_index = mission.current_index; -// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); -// } else { -// _current_mission_index = 0; -// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); -// } - _mission_item_count = mission.count; _current_mission_index = mission.current_index; @@ -385,7 +370,7 @@ Navigator::mission_update() _mission_item_count = 0; _current_mission_index = 0; } - if (_mission_item_count == 0 && _mode == NAVIGATION_MODE_WAYPOINT) { + if ((!mission_possible() || !onboard_mission_possible()) && _mode == NAVIGATION_MODE_WAYPOINT) { set_mode(NAVIGATION_MODE_LOITER); } else if (_mode == NAVIGATION_MODE_WAYPOINT) { @@ -393,7 +378,27 @@ 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) { + _onboard_mission_item_count = onboard_mission.count; + _current_onboard_mission_index = onboard_mission.current_index; + + } else { + _onboard_mission_item_count = 0; + _current_onboard_mission_index = 0; + } + + if ((!mission_possible() || !onboard_mission_possible()) && _mode == NAVIGATION_MODE_WAYPOINT) { + set_mode(NAVIGATION_MODE_LOITER); + } + else if (_mode == NAVIGATION_MODE_WAYPOINT) { + start_waypoint(); + } +} void Navigator::task_main_trampoline(int argc, char *argv[]) @@ -414,6 +419,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)); @@ -425,6 +431,7 @@ Navigator::task_main() } mission_update(); + onboard_mission_update(); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -439,7 +446,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; @@ -452,8 +459,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) { @@ -475,7 +484,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); @@ -505,8 +514,8 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_MISSION: - if (_mission_item_count > 0 && !(_current_mission_index >= _mission_item_count)) { - /* Start mission if there is a mission available and the last waypoint has not been reached */ + if (mission_possible() || onboard_mission_possible()) { + /* Start mission or onboard mission if available */ set_mode(NAVIGATION_MODE_WAYPOINT); } else { /* else fallback to loiter */ @@ -556,6 +565,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); } @@ -586,9 +599,15 @@ Navigator::task_main() if (_mission_item_reached) { - report_mission_reached(); + + + if (onboard_mission_possible()) { + mavlink_log_info(_mavlink_fd, "[navigator] reached onboard WP %d", _current_onboard_mission_index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index); + report_mission_reached(); + } - mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index); if (advance_current_mission_item() != OK) { set_mode(NAVIGATION_MODE_LOITER_WAYPOINT); } @@ -863,16 +882,27 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) } } -int -Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) +bool +Navigator::mission_possible() +{ + return _mission_item_count > 0 && + !(_current_mission_index >= _mission_item_count); +} + +bool +Navigator::onboard_mission_possible() +{ + return _onboard_mission_item_count > 0 && + !(_current_onboard_mission_index >= _onboard_mission_item_count) && + _parameters.onboard_mission_enabled; +} + +int +Navigator::set_waypoint_mission_item(dm_item_t dm_item, unsigned mission_item_index, struct mission_item_s *new_mission_item) { - if (mission_item_index >= _mission_item_count) { - return ERROR; - } - struct mission_item_s mission_item; - - if (dm_read(DM_KEY_WAYPOINTS, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + + if (dm_read(dm_item, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { return ERROR; } @@ -926,8 +956,7 @@ Navigator::advance_current_mission_item() // warnx("advancing from %d to %d", _current_mission_index, _current_mission_index+1); /* ultimately this index will be == _mission_item_count and this flags the mission as completed */ - _current_mission_index++; - + /* if there is no more mission available, don't advance and return */ if (!_mission_item_triplet.next_valid) { // warnx("no next available"); @@ -941,9 +970,20 @@ Navigator::advance_current_mission_item() /* 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; + + int ret = ERROR; + + if (onboard_mission_possible()) { + _current_onboard_mission_index++; + ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, &_mission_item_triplet.next); + } else if (mission_possible()) { + _current_mission_index++; + ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index + 1, &_mission_item_triplet.next); + } else { + warnx("Error: nothing to advance"); + } - - if(set_waypoint_mission_item(_current_mission_index + 1, &_mission_item_triplet.next) == OK) { + if(ret == OK) { _mission_item_triplet.next_valid = true; } else { @@ -1078,17 +1118,27 @@ Navigator::start_waypoint() { reset_mission_item_reached(); - if (_current_mission_index > 0) { - set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous); - _mission_item_triplet.previous_valid = true; - } else { - _mission_item_triplet.previous_valid = false; - } + // if (_current_mission_index > 0) { + // set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous); + // _mission_item_triplet.previous_valid = true; + // } else { + // _mission_item_triplet.previous_valid = false; + // } + _mission_item_triplet.previous_valid = false; - set_waypoint_mission_item(_current_mission_index, &_mission_item_triplet.current); - _mission_item_triplet.current_valid = true; + int ret = ERROR; - mavlink_log_info(_mavlink_fd, "[navigator] heading to WP %d", _current_mission_index); + if (onboard_mission_possible()) { + set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, &_mission_item_triplet.current); + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", _current_onboard_mission_index); + ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, &_mission_item_triplet.next); + } else if (mission_possible()) { + set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index, &_mission_item_triplet.current); + mavlink_log_info(_mavlink_fd, "[navigator] heading to WP %d", _current_mission_index); + ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index + 1, &_mission_item_triplet.next); + } + + _mission_item_triplet.current_valid = true; // if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { @@ -1100,7 +1150,7 @@ Navigator::start_waypoint() // _mission_item_triplet.next_valid = true; // } - if(set_waypoint_mission_item(_current_mission_index + 1, &_mission_item_triplet.next) == OK) { + if(ret == OK) { _mission_item_triplet.next_valid = true; } else { diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 30f06c3597..3702420076 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -96,7 +96,7 @@ struct mission_item_s struct mission_s { - unsigned count; + unsigned count; /**< count of the missions stored in the datamanager */ int current_index; /**< default -1, start at the one changed latest */ }; @@ -106,5 +106,6 @@ struct mission_s /* register this as object request broker structure */ ORB_DECLARE(mission); +ORB_DECLARE(onboard_mission); #endif From 6a624abd7c8fe9cf32b5f5a91bceeb93f730a0ec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:54:25 +0100 Subject: [PATCH 5/7] Datamanager: Rename mavlink/offboard key --- src/modules/dataman/dataman.c | 2 +- src/modules/dataman/dataman.h | 4 ++-- src/modules/mavlink/waypoints.c | 6 +++--- src/systemcmds/tests/test_dataman.c | 8 ++++---- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index acd612d9e9..874a47be7f 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -111,7 +111,7 @@ static unsigned g_func_counts[dm_number_of_funcs]; static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_SAFE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX, - DM_KEY_WAYPOINTS_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_MAX, DM_KEY_WAYPOINTS_ONBOARD_MAX }; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index dab96eb9b1..2a781405aa 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -50,7 +50,7 @@ extern "C" { typedef enum { DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAYPOINTS, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_OFFBOARD, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -59,7 +59,7 @@ extern "C" { enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_OFFBOARD_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED }; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 7aad5038d2..52a580d5be 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -701,7 +701,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi struct mission_item_s mission_item; ssize_t len = sizeof(struct mission_item_s); - if (dm_read(DM_KEY_WAYPOINTS, wpr.seq, &mission_item, len) == len) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, wpr.seq, &mission_item, len) == len) { if (mission.current_index == wpr.seq) { wp.current = true; @@ -975,7 +975,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi size_t len = sizeof(struct mission_item_s); - if (dm_write(DM_KEY_WAYPOINTS, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + if (dm_write(DM_KEY_WAYPOINTS_OFFBOARD, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; @@ -1117,7 +1117,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi /* prepare mission topic */ mission.count = 0; - if (dm_clear(DM_KEY_WAYPOINTS) == OK) { + if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD) == OK) { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index 7de87b476f..5b121e34ed 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -89,7 +89,7 @@ task_main(int argc, char *argv[]) unsigned hash = i ^ my_id; unsigned len = (hash & 63) + 2; - int ret = dm_write(DM_KEY_WAYPOINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); + int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); warnx("ret: %d", ret); if (ret != len) { warnx("%d write failed, index %d, length %d", my_id, hash, len); @@ -103,7 +103,7 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { unsigned hash = i ^ my_id; unsigned len2, len = (hash & 63) + 2; - if ((len2 = dm_read(DM_KEY_WAYPOINTS, hash, buffer, sizeof(buffer))) < 2) { + if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD, hash, buffer, sizeof(buffer))) < 2) { warnx("%d read failed length test, index %d", my_id, hash); goto fail; } @@ -163,7 +163,7 @@ int test_dataman(int argc, char *argv[]) free(sems); dm_restart(DM_INIT_REASON_IN_FLIGHT); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) break; } if (i >= NUM_MISSIONS_SUPPORTED) { @@ -173,7 +173,7 @@ int test_dataman(int argc, char *argv[]) } dm_restart(DM_INIT_REASON_POWER_ON); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) { warnx("Restart power-on failed"); return -1; } From 624ae85efa078ddd63209fe53c2c215986116ad1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:55:03 +0100 Subject: [PATCH 6/7] Navigator: Use state table for main FSM --- src/modules/navigator/navigator_main.cpp | 1089 ++++++++++++---------- src/modules/systemlib/state_table.h | 75 ++ 2 files changed, 680 insertions(+), 484 deletions(-) create mode 100644 src/modules/systemlib/state_table.h diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e2e2949e25..d258aa8a69 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -38,6 +38,7 @@ * Implementation of the main navigation state machine. * * Handles missions, geo fencing and failsafe navigation behavior. + * Published the mission item triplet for the position controller. */ #include @@ -66,21 +67,15 @@ #include #include #include -#include +#include #include #include +#include #include #include #include -typedef enum { - NAVIGATION_MODE_NONE, - NAVIGATION_MODE_LOITER, - NAVIGATION_MODE_WAYPOINT, - NAVIGATION_MODE_LOITER_WAYPOINT, - NAVIGATION_MODE_RTL, - NAVIGATION_MODE_LOITER_RTL -} navigation_mode_t; + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -95,7 +90,7 @@ static const int ERROR = -1; */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); -class Navigator +class Navigator : public StateTable { public: /** @@ -141,7 +136,7 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ - int _mission_sub; /**< notification of mission updates */ + int _offboard_mission_sub; /**< notification of offboard mission updates */ int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ @@ -157,22 +152,26 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - unsigned _mission_item_count; /** number of mission items copied */ - unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ - struct fence_s _fence; /**< storage for fence vertices */ + struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ struct navigation_capabilities_s _nav_caps; + unsigned _current_offboard_mission_index; + unsigned _current_onboard_mission_index; + unsigned _offboard_mission_item_count; /** number of offboard mission items copied */ + unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_ONBOARD, + MISSION_TYPE_OFFBOARD, + } _mission_type; + bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _mission_item_reached; - - navigation_mode_t _mode; - unsigned _current_mission_index; - unsigned _current_onboard_mission_index; struct { float min_altitude; @@ -187,22 +186,69 @@ private: } _parameter_handles; /**< handles for parameters */ + enum Event { + EVENT_NONE_REQUESTED, + EVENT_LOITER_REQUESTED, + EVENT_MISSION_REQUESTED, + EVENT_RTL_REQUESTED, + EVENT_MISSION_FINISHED, + EVENT_MISSION_CHANGED, + EVENT_HOME_POSITION_CHANGED, + MAX_EVENT + }; + + enum State { + STATE_INIT, + STATE_NONE, + STATE_LOITER, + STATE_MISSION, + STATE_MISSION_LOITER, + STATE_RTL, + STATE_RTL_LOITER, + MAX_STATE + }; + + /** + * State machine transition table + */ + static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT]; /** * Update our local parameter cache. */ - int parameters_update(); + void parameters_update(); /** - * Retrieve mission. - */ - void mission_update(); + * Retrieve global position + */ + void global_position_update(); /** - * Retrieve onboard mission. - */ + * Retrieve home position + */ + void home_position_update(); + + /** + * Retreive navigation capabilities + */ + void navigation_capabilities_update(); + + /** + * Retrieve offboard mission. + */ + void offboard_mission_update(); + + /** + * Retrieve onboard mission. + */ void onboard_mission_update(); + /** + * Retrieve vehicle status + */ + void vehicle_status_update(); + + /** * Shim for calling task_main from task_create. */ @@ -219,31 +265,52 @@ private: bool fence_valid(const struct fence_s &fence); - void set_mode(navigation_mode_t new_nav_mode); + /** + * Functions that are triggered when a new state is entered. + */ + void start_none(); + void start_loiter(); + void start_mission(); + void start_mission_loiter(); + void start_rtl(); + void start_rtl_loiter(); - bool mission_possible(); + /** + * Guards offboard mission + */ + bool offboard_mission_available(unsigned relative_index); - bool onboard_mission_possible(); + /** + * Guards onboard mission + */ + bool onboard_mission_available(unsigned relative_index); - int set_waypoint_mission_item(dm_item_t dm_item, unsigned mission_item_index, struct mission_item_s *new_mission_item); + /** + * Check if current mission item has been reached. + */ + bool mission_item_reached(); + /** + * Move to next waypoint + */ + int advance_mission(); + + /** + * Helper function to set a mission item + */ + int set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) ; + + /** + * Helper function to set a loiter item + */ + void set_loiter_item(mission_item_s *new_loiter_position); + + /** + * Publish a new mission item triplet for position controller + */ void publish_mission_item_triplet(); - void publish_mission_result(); - int advance_current_mission_item(); - - void reset_mission_item_reached(); - - void check_mission_item_reached(); - - void report_mission_reached(); - - void start_waypoint(); - - void start_loiter(mission_item_s *new_loiter_position); - - void start_rtl(); /** * Compare two mission items if they are equivalent @@ -266,11 +333,13 @@ static const int ERROR = -1; Navigator *g_navigator; } -Navigator::Navigator() : +Navigator::Navigator() : + +/* state machine transition table */ + StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), - _mavlink_fd(-1), /* subscriptions */ @@ -278,7 +347,7 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _params_sub(-1), - _mission_sub(-1), + _offboard_mission_sub(-1), _onboard_mission_sub(-1), _capabilities_sub(-1), @@ -289,21 +358,20 @@ Navigator::Navigator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), + /* states */ - _mission_item_count(0), - _onboard_mission_item_count(0), _fence_valid(false), _inside_fence(true), + _current_offboard_mission_index(0), + _current_onboard_mission_index(0), + _offboard_mission_item_count(0), + _onboard_mission_item_count(0), _waypoint_position_reached(false), _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), - _mission_item_reached(false), - _mode(NAVIGATION_MODE_NONE), - _current_mission_index(0), - _current_onboard_mission_index(0) + _time_first_inside_orbit(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"); @@ -317,8 +385,8 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); - /* fetch initial values */ - parameters_update(); + /* Initialize state machine */ + myState = STATE_INIT; } Navigator::~Navigator() @@ -346,35 +414,53 @@ Navigator::~Navigator() navigator::g_navigator = nullptr; } -int +void Navigator::parameters_update() { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &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; } void -Navigator::mission_update() +Navigator::global_position_update() { - struct mission_s mission; - if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { + /* load local copies */ + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); +} - _mission_item_count = mission.count; - _current_mission_index = mission.current_index; +void +Navigator::home_position_update() +{ + orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); +} + +void +Navigator::navigation_capabilities_update() +{ + orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); +} + + +void +Navigator::offboard_mission_update() +{ + struct mission_s offboard_mission; + if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + + _offboard_mission_item_count = offboard_mission.count; + + if (offboard_mission.current_index != -1) { + _current_offboard_mission_index = offboard_mission.current_index; + } } else { - _mission_item_count = 0; - _current_mission_index = 0; - } - if ((!mission_possible() || !onboard_mission_possible()) && _mode == NAVIGATION_MODE_WAYPOINT) { - set_mode(NAVIGATION_MODE_LOITER); - } - else if (_mode == NAVIGATION_MODE_WAYPOINT) { - start_waypoint(); + _offboard_mission_item_count = 0; + _current_offboard_mission_index = 0; } } @@ -385,18 +471,23 @@ Navigator::onboard_mission_update() if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _onboard_mission_item_count = onboard_mission.count; - _current_onboard_mission_index = onboard_mission.current_index; + + if (onboard_mission.current_index != -1) { + _current_onboard_mission_index = onboard_mission.current_index; + } } else { _onboard_mission_item_count = 0; _current_onboard_mission_index = 0; } +} - if ((!mission_possible() || !onboard_mission_possible()) && _mode == NAVIGATION_MODE_WAYPOINT) { - set_mode(NAVIGATION_MODE_LOITER); - } - else if (_mode == NAVIGATION_MODE_WAYPOINT) { - start_waypoint(); +void +Navigator::vehicle_status_update() +{ + /* try to load initial states */ + if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { + _vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */ } } @@ -414,37 +505,34 @@ Navigator::task_main() warnx("Initializing.."); fflush(stdout); + _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); + /* * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _mission_sub = orb_subscribe(ORB_ID(mission)); + _offboard_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)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - // Load initial states - if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { - _vstatus.arming_state = ARMING_STATE_STANDBY; // for testing... commander may not be running - } - - mission_update(); + + /* copy all topics first time */ + parameters_update(); + global_position_update(); + home_position_update(); + navigation_capabilities_update(); + offboard_mission_update(); onboard_mission_update(); + vehicle_status_update(); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - parameters_update(); - - _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); - - /* load the craft capabilities */ - orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); - /* wakeup source(s) */ struct pollfd fds[7]; @@ -457,7 +545,7 @@ Navigator::task_main() fds[2].events = POLLIN; fds[3].fd = _capabilities_sub; fds[3].events = POLLIN; - fds[4].fd = _mission_sub; + fds[4].fd = _offboard_mission_sub; fds[4].events = POLLIN; fds[5].fd = _onboard_mission_sub; fds[5].events = POLLIN; @@ -485,8 +573,8 @@ Navigator::task_main() /* only update vehicle status if it changed */ if (fds[6].revents & POLLIN) { - /* read from param to clear updated flag */ - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + + vehicle_status_update(); /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO) { @@ -497,135 +585,96 @@ Navigator::task_main() case NAVIGATION_STATE_ALTHOLD: case NAVIGATION_STATE_VECTOR: - set_mode(NAVIGATION_MODE_NONE); + /* don't publish a mission item triplet */ + dispatch(EVENT_NONE_REQUESTED); break; case NAVIGATION_STATE_AUTO_READY: case NAVIGATION_STATE_AUTO_TAKEOFF: + case NAVIGATION_STATE_AUTO_MISSION: - /* TODO probably not needed since takeoff WPs will just be passed on */ - //set_mode(NAVIGATION_MODE_TAKEOFF); + /* try mission if none is available, fallback to loiter instead */ + if (onboard_mission_available(0) || offboard_mission_available(0)) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } break; case NAVIGATION_STATE_AUTO_LOITER: - set_mode(NAVIGATION_MODE_LOITER); + dispatch(EVENT_LOITER_REQUESTED); break; - case NAVIGATION_STATE_AUTO_MISSION: - - if (mission_possible() || onboard_mission_possible()) { - /* Start mission or onboard mission if available */ - set_mode(NAVIGATION_MODE_WAYPOINT); - } else { - /* else fallback to loiter */ - set_mode(NAVIGATION_MODE_LOITER); - } - break; case NAVIGATION_STATE_AUTO_RTL: - set_mode(NAVIGATION_MODE_RTL); + dispatch(EVENT_RTL_REQUESTED); break; case NAVIGATION_STATE_AUTO_LAND: /* TODO add this */ - //set_mode(NAVIGATION_MODE_LAND); + break; default: - warnx("Navigation state not supported"); + warnx("ERROR: Navigation state not supported"); break; } } else { - set_mode(NAVIGATION_MODE_NONE); + /* not in AUTO */ + dispatch(EVENT_NONE_REQUESTED); + } + + /* XXX Hack to get mavlink output going, try opening the fd with 5Hz */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } } + /* only update parameters if it changed */ if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - /* update parameters from storage */ parameters_update(); - /* note that these new parameters won't be in effect until a mission triplet is published again */ } /* only update craft capabilities if they have changed */ if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + navigation_capabilities_update(); } if (fds[4].revents & POLLIN) { - mission_update(); + offboard_mission_update(); + // XXX check if mission really changed + dispatch(EVENT_MISSION_CHANGED); } if (fds[5].revents & POLLIN) { onboard_mission_update(); + // XXX check if mission really changed + dispatch(EVENT_MISSION_CHANGED); } if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + home_position_update(); + // XXX check if home position really changed + dispatch(EVENT_HOME_POSITION_CHANGED); } /* only run controller if position changed */ if (fds[1].revents & POLLIN) { - - /* XXX Hack to get mavlink output going */ - if (_mavlink_fd < 0) { - /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - } - - /* load local copies */ - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); - - /* do stuff according to mode */ - switch (_mode) { - case NAVIGATION_MODE_NONE: - case NAVIGATION_MODE_LOITER: - case NAVIGATION_MODE_LOITER_WAYPOINT: - case NAVIGATION_MODE_LOITER_RTL: - break; - - case NAVIGATION_MODE_WAYPOINT: - - check_mission_item_reached(); - - if (_mission_item_reached) { - - - - if (onboard_mission_possible()) { - mavlink_log_info(_mavlink_fd, "[navigator] reached onboard WP %d", _current_onboard_mission_index); - } else { - mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index); - report_mission_reached(); - } - - if (advance_current_mission_item() != OK) { - set_mode(NAVIGATION_MODE_LOITER_WAYPOINT); - } - } - break; - - case NAVIGATION_MODE_RTL: - - check_mission_item_reached(); - - if (_mission_item_reached) { - mavlink_log_info(_mavlink_fd, "[navigator] reached RTL position"); - set_mode(NAVIGATION_MODE_LOITER_RTL); - } - break; - default: - warnx("navigation mode not supported"); - break; + global_position_update(); + /* only check if waypoint has been reached in Mission or RTL mode */ + if (mission_item_reached()) { + /* try to advance mission */ + if (advance_mission() != OK) { + dispatch(EVENT_MISSION_FINISHED); + } } } perf_end(_loop_perf); @@ -668,15 +717,57 @@ Navigator::status() (double)_global_pos.alt, (double)_global_pos.relative_alt); warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); - warnx("Compass heading in degrees %5.5f", (double)_global_pos.yaw * 57.2957795); + warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } if (_fence_valid) { warnx("Geofence is valid"); warnx("Vertex longitude latitude"); for (unsigned i = 0; i < _fence.count; i++) warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); - } else + } else { warnx("Geofence not set"); + } + + switch (myState) { + case STATE_INIT: + warnx("State: Init"); + break; + case STATE_NONE: + warnx("State: None"); + break; + case STATE_LOITER: + warnx("State: Loiter"); + break; + case STATE_MISSION: + warnx("State: Mission"); + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + warnx("Mission type: Onboard"); + break; + case MISSION_TYPE_OFFBOARD: + warnx("Mission type: Offboard"); + break; + case MISSION_TYPE_NONE: + default: + warnx("ERROR: Mission type unsupported"); + break; + } + warnx("Onboard mission: %d of %d", _current_onboard_mission_index, _onboard_mission_item_count); + warnx("Offboard mission: %d of %d", _current_offboard_mission_index, _offboard_mission_item_count); + break; + case STATE_MISSION_LOITER: + warnx("State: Loiter after Mission"); + break; + case STATE_RTL: + warnx("State: RTL"); + break; + case STATE_RTL_LOITER: + warnx("State: Loiter after RTL"); + break; + default: + warnx("State: Unknown"); + break; + } } void @@ -767,199 +858,177 @@ Navigator::fence_point(int argc, char *argv[]) errx(1, "can't store fence point"); } + + +StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { + { + /* STATE_INIT */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_INIT}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_INIT}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_INIT}, + }, + { + /* STATE_NONE */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_NONE}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE}, + }, + { + /* STATE_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER}, + }, + { + /* STATE_MISSION */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION}, + }, + { + /* STATE_MISSION_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION_LOITER}, + }, + { + /* STATE_RTL */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_RTL}, + }, + { + /* STATE_RTL_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + }, +}; + void -Navigator::set_mode(navigation_mode_t new_nav_mode) +Navigator::start_none() { - if (new_nav_mode == _mode) { - /* no change, return */ - return; + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = false; + _mission_item_triplet.next_valid = false; + + publish_mission_item_triplet(); +} + +void +Navigator::start_loiter() +{ + struct mission_item_s loiter_item; + + loiter_item.lat = (double)_global_pos.lat / 1e7; + loiter_item.lon = (double)_global_pos.lon / 1e7; + loiter_item.yaw = 0.0f; + + /* XXX get rid of ugly conversion for home position altitude */ + float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; + + /* Use current altitude if above min altitude set by parameter */ + if (_global_pos.alt < global_min_alt) { + loiter_item.altitude = global_min_alt; + mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); + } else { + loiter_item.altitude = _global_pos.alt; + mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); } - switch (new_nav_mode) { - case NAVIGATION_MODE_NONE: + set_loiter_item(&loiter_item); - // warnx("Set mode NONE"); - _mode = new_nav_mode; + publish_mission_item_triplet(); +} + + +void +Navigator::start_mission() +{ + /* leave previous mission item as isas is */ + + if(set_mission_item(0, &_mission_item_triplet.current) == OK) { + _mission_item_triplet.current_valid = true; + } else { + _mission_item_triplet.current_valid = false; + warnx("ERROR: current WP can't be set"); + } + + if(set_mission_item(1, &_mission_item_triplet.next) == OK) { + _mission_item_triplet.next_valid = true; + } else { + _mission_item_triplet.next_valid = false; + } + + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", _current_onboard_mission_index); break; - - case NAVIGATION_MODE_LOITER: - - /* decide depending on previous navigation mode */ - switch (_mode) { - case NAVIGATION_MODE_NONE: - - case NAVIGATION_MODE_WAYPOINT: - case NAVIGATION_MODE_RTL: { - - /* use current position and loiter around it */ - mission_item_s global_position_mission_item; - global_position_mission_item.lat = (double)_global_pos.lat / 1e7; - global_position_mission_item.lon = (double)_global_pos.lon / 1e7; - - /* XXX get rid of ugly conversion for home position altitude */ - float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; - - /* Use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < global_min_alt) { - global_position_mission_item.altitude = global_min_alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); - } else { - global_position_mission_item.altitude = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); - } - start_loiter(&global_position_mission_item); - _mode = new_nav_mode; - - break; - } - case NAVIGATION_MODE_LOITER_WAYPOINT: - case NAVIGATION_MODE_LOITER_RTL: - /* already loitering, just continue */ - _mode = new_nav_mode; - // warnx("continue loiter"); - break; - - case NAVIGATION_MODE_LOITER: - default: - // warnx("previous navigation mode not supported"); - break; - } + case MISSION_TYPE_OFFBOARD: + mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", _current_offboard_mission_index); break; - - case NAVIGATION_MODE_WAYPOINT: - - /* Start mission if there is one available */ - start_waypoint(); - _mode = new_nav_mode; - mavlink_log_info(_mavlink_fd, "[navigator] start waypoint mission"); - break; - - case NAVIGATION_MODE_LOITER_WAYPOINT: - - start_loiter(&_mission_item_triplet.current); - _mode = new_nav_mode; - mavlink_log_info(_mavlink_fd, "[navigator] loiter at WP %d", _current_mission_index-1); - break; - - case NAVIGATION_MODE_RTL: - - /* decide depending on previous navigation mode */ - switch (_mode) { - case NAVIGATION_MODE_NONE: - case NAVIGATION_MODE_LOITER: - case NAVIGATION_MODE_WAYPOINT: - case NAVIGATION_MODE_LOITER_WAYPOINT: - - /* start RTL here */ - start_rtl(); - _mode = new_nav_mode; - mavlink_log_info(_mavlink_fd, "[navigator] start RTL"); - break; - - case NAVIGATION_MODE_LOITER_RTL: - /* already loitering after RTL, just continue */ - // warnx("stay in loiter after RTL"); - break; - - case NAVIGATION_MODE_RTL: - default: - warnx("previous navigation mode not supported"); - break; - } - break; - - case NAVIGATION_MODE_LOITER_RTL: - - /* TODO: get rid of this conversion */ - mission_item_s home_position_mission_item; - home_position_mission_item.lat = (double)_home_pos.lat / 1e7; - home_position_mission_item.lon = (double)_home_pos.lon / 1e7; - home_position_mission_item.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; - start_loiter(&home_position_mission_item); - mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); - _mode = new_nav_mode; + case MISSION_TYPE_NONE: + default: + warnx("ERROR: Mission type unsupported"); break; } + + publish_mission_item_triplet(); } -bool -Navigator::mission_possible() -{ - return _mission_item_count > 0 && - !(_current_mission_index >= _mission_item_count); -} -bool -Navigator::onboard_mission_possible() -{ - return _onboard_mission_item_count > 0 && - !(_current_onboard_mission_index >= _onboard_mission_item_count) && - _parameters.onboard_mission_enabled; -} int -Navigator::set_waypoint_mission_item(dm_item_t dm_item, unsigned mission_item_index, struct mission_item_s *new_mission_item) +Navigator::advance_mission() { - struct mission_item_s mission_item; - - if (dm_read(dm_item, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - return ERROR; + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + warnx("advance onboard before: %d", _current_onboard_mission_index); + _current_onboard_mission_index++; + warnx("advance onboard after: %d", _current_onboard_mission_index); + break; + case MISSION_TYPE_OFFBOARD: + warnx("advance offboard before: %d", _current_offboard_mission_index); + _current_offboard_mission_index++; + warnx("advance offboard after: %d", _current_offboard_mission_index); + break; + case MISSION_TYPE_NONE: + default: + warnx("ERROR: Mission type unsupported"); + return ERROR; } - memcpy(new_mission_item, &mission_item, sizeof(struct mission_item_s)); - - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = (double)_home_pos.lat / 1e7; - new_mission_item->lon = (double)_home_pos.lon / 1e7; - new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; - new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_mission_item->radius = 50.0f; // TODO: get rid of magic number - } - - return OK; -} - -void -Navigator::publish_mission_item_triplet() -{ - /* lazily publish the mission triplet only once available */ - if (_triplet_pub > 0) { - /* publish the mission triplet */ - 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); - } -} - -void -Navigator::publish_mission_result() -{ - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish the mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); - - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); - } -} - -int -Navigator::advance_current_mission_item() -{ - reset_mission_item_reached(); - - // warnx("advancing from %d to %d", _current_mission_index, _current_mission_index+1); - - /* ultimately this index will be == _mission_item_count and this flags the mission as completed */ - - /* if there is no more mission available, don't advance and return */ + /* if there is no more mission available, don't advance and switch to loiter at current WP */ if (!_mission_item_triplet.next_valid) { - // warnx("no next available"); + warnx("no next valid"); return ERROR; } @@ -967,62 +1036,182 @@ Navigator::advance_current_mission_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; - - int ret = ERROR; - - if (onboard_mission_possible()) { - _current_onboard_mission_index++; - ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, &_mission_item_triplet.next); - } else if (mission_possible()) { - _current_mission_index++; - ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index + 1, &_mission_item_triplet.next); + if(set_mission_item(0, &_mission_item_triplet.current) == OK) { + _mission_item_triplet.current_valid = true; + } else { - warnx("Error: nothing to advance"); + /* should never ever happen */ + _mission_item_triplet.current_valid = false; + warnx("no current available"); + return ERROR; } - if(ret == OK) { + if(set_mission_item(1, &_mission_item_triplet.next) == OK) { _mission_item_triplet.next_valid = true; - } - else { + + } else { _mission_item_triplet.next_valid = false; } publish_mission_item_triplet(); - return OK; } - void -Navigator::reset_mission_item_reached() +Navigator::start_mission_loiter() { - /* reset all states */ - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - _time_first_inside_orbit = 0; + /* make sure the current WP is valid */ + if (!_mission_item_triplet.current_valid) { + warnx("ERROR: cannot switch to offboard mission loiter"); + return; + } - _mission_item_reached = false; + set_loiter_item(&_mission_item_triplet.current); - _mission_result.mission_reached = false; - _mission_result.mission_index = 0; + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + mavlink_log_info(_mavlink_fd, "[navigator] loiter at onboard WP %d", _current_onboard_mission_index-1); + break; + case MISSION_TYPE_OFFBOARD: + mavlink_log_info(_mavlink_fd, "[navigator] loiter at offboard WP %d", _current_offboard_mission_index-1); + break; + case MISSION_TYPE_NONE: + default: + warnx("ERROR: Mission type unsupported"); + break; + } } void -Navigator::check_mission_item_reached() +Navigator::start_rtl() { - /* don't check if mission item is already reached */ - if (_mission_item_reached) { - return; + + /* discard all mission item and insert RTL item */ + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; + _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; + _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + _mission_item_triplet.current.yaw = 0.0f; + _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number + _mission_item_triplet.current.autocontinue = false; + _mission_item_triplet.current_valid = true; + + publish_mission_item_triplet(); + + mavlink_log_info(_mavlink_fd, "[navigator] return to launch"); +} + + +void +Navigator::start_rtl_loiter() +{ + mission_item_s home_position_mission_item; + home_position_mission_item.lat = (double)_home_pos.lat / 1e7; + home_position_mission_item.lon = (double)_home_pos.lon / 1e7; + home_position_mission_item.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; + + set_loiter_item(&home_position_mission_item); + + mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); +} + +bool +Navigator::offboard_mission_available(unsigned relative_index) +{ + return _offboard_mission_item_count > _current_offboard_mission_index + relative_index; +} + +bool +Navigator::onboard_mission_available(unsigned relative_index) +{ + return _onboard_mission_item_count > _current_onboard_mission_index + relative_index && _parameters.onboard_mission_enabled; +} + +int +Navigator::set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) +{ + struct mission_item_s new_mission_item; + + /* try onboard mission first */ + if (onboard_mission_available(relative_index)) { + if (_mission_type != MISSION_TYPE_ONBOARD && relative_index == 1) { + relative_index--; + } + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + relative_index, &new_mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + _mission_type = MISSION_TYPE_NONE; + return ERROR; + } + /* base the mission type on the current mission item, not on future ones */ + if (relative_index == 0) { + _mission_type = MISSION_TYPE_ONBOARD; + } + /* otherwise fallback to offboard */ + } else if (offboard_mission_available(relative_index)) { + + warnx("fallback try offboard: %d / %d", _current_offboard_mission_index + relative_index, _offboard_mission_item_count); + + if (_mission_type != MISSION_TYPE_OFFBOARD && relative_index == 1) { + relative_index--; + } + + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + relative_index, &new_mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + _mission_type = MISSION_TYPE_NONE; + warnx("failed"); + return ERROR; + } + /* base the mission type on the current mission item, not on future ones */ + if (relative_index == 0) { + _mission_type = MISSION_TYPE_OFFBOARD; + } + } else { + /* happens when no more mission items can be added as a next item */ + return ERROR; } - /* don't try to reach the landing mission, just stay in that mode */ - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - return; + if (new_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* if it is a RTL waypoint, append the home position */ + new_mission_item.lat = (double)_home_pos.lat / 1e7; + new_mission_item.lon = (double)_home_pos.lon / 1e7; + new_mission_item.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_mission_item.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_mission_item.radius = 50.0f; // TODO: get rid of magic number } + memcpy(mission_item, &new_mission_item, sizeof(struct mission_item_s)); + + return OK; +} + +bool +Navigator::mission_item_reached() +{ + /* only check if there is actually a mission item to check */ + if (!_mission_item_triplet.current_valid) { + return false; + } + + /* don't try to reach the landing mission, just stay in that mode, XXX maybe add another state for this */ + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + return false; + } + + /* XXX TODO count turns */ + if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item_triplet.current.loiter_radius > 0.01f) { + + return false; + } + uint64_t now = hrt_absolute_time(); float orbit; @@ -1030,12 +1219,6 @@ Navigator::check_mission_item_reached() orbit = _mission_item_triplet.current.radius; - } else if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item_triplet.current.loiter_radius > 0.01f) { - - orbit = _mission_item_triplet.current.loiter_radius; } else { // XXX set default orbit via param @@ -1098,73 +1281,19 @@ Navigator::check_mission_item_reached() if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - _mission_item_reached = true; + _time_first_inside_orbit = 0; + _waypoint_yaw_reached = false; + _waypoint_position_reached = false; + return true; } } + return false; } void -Navigator::report_mission_reached() +Navigator::set_loiter_item(struct mission_item_s *new_loiter_position) { - _mission_result.mission_reached = true; - _mission_result.mission_index = _current_mission_index; - - publish_mission_result(); -} - -void -Navigator::start_waypoint() -{ - reset_mission_item_reached(); - - // if (_current_mission_index > 0) { - // set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous); - // _mission_item_triplet.previous_valid = true; - // } else { - // _mission_item_triplet.previous_valid = false; - // } - _mission_item_triplet.previous_valid = false; - - int ret = ERROR; - - if (onboard_mission_possible()) { - set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, &_mission_item_triplet.current); - mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", _current_onboard_mission_index); - ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, &_mission_item_triplet.next); - } else if (mission_possible()) { - set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index, &_mission_item_triplet.current); - mavlink_log_info(_mavlink_fd, "[navigator] heading to WP %d", _current_mission_index); - ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index + 1, &_mission_item_triplet.next); - } - - _mission_item_triplet.current_valid = true; - - // if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - - // if the current mission is to loiter unlimited, don't bother about a next mission item - // _mission_item_triplet.next_valid = false; - // } else { - /* if we are not loitering yet, try to add the next mission item */ - // set_waypoint_mission_item(_current_mission_index + 1, &_mission_item_triplet.next); - // _mission_item_triplet.next_valid = true; - // } - - if(ret == OK) { - _mission_item_triplet.next_valid = true; - } - else { - _mission_item_triplet.next_valid = false; - } - - publish_mission_item_triplet(); -} - -void -Navigator::start_loiter(mission_item_s *new_loiter_position) -{ - //reset_mission_item_reached(); - _mission_item_triplet.previous_valid = false; _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; @@ -1184,27 +1313,38 @@ Navigator::start_loiter(mission_item_s *new_loiter_position) } void -Navigator::start_rtl() +Navigator::publish_mission_item_triplet() { - reset_mission_item_reached(); + /* lazily publish the mission triplet only once available */ + if (_triplet_pub > 0) { + /* publish the mission triplet */ + orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); - /* discard all mission item and insert RTL item */ - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + } else { + /* advertise and publish */ + _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); + } +} - _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; - _mission_item_triplet.current.yaw = 0.0f; - _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number - _mission_item_triplet.current.autocontinue = false; - _mission_item_triplet.current_valid = true; - publish_mission_item_triplet(); +bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { + if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && + fabsf(a.lat - b.lat) < FLT_EPSILON && + fabsf(a.lon - b.lon) < FLT_EPSILON && + fabsf(a.altitude - b.altitude) < FLT_EPSILON && + fabsf(a.yaw - b.yaw) < FLT_EPSILON && + fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && + fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON && + fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON && + fabsf(a.radius - b.radius) < FLT_EPSILON && + fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && + fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON && + fabsf(a.index - b.index) < FLT_EPSILON) { + return true; + } else { + warnx("a.index %d, b.index %d", a.index, b.index); + return false; + } } @@ -1260,22 +1400,3 @@ int navigator_main(int argc, char *argv[]) return 0; } -bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { - if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && - fabsf(a.lat - b.lat) < FLT_EPSILON && - fabsf(a.lon - b.lon) < FLT_EPSILON && - fabsf(a.altitude - b.altitude) < FLT_EPSILON && - fabsf(a.yaw - b.yaw) < FLT_EPSILON && - fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && - fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON && - fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON && - fabsf(a.radius - b.radius) < FLT_EPSILON && - fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && - fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON && - fabsf(a.index - b.index) < FLT_EPSILON) { - return true; - } else { - warnx("a.index %d, b.index %d", a.index, b.index); - return false; - } -} diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h new file mode 100644 index 0000000000..f2709d29ff --- /dev/null +++ b/src/modules/systemlib/state_table.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file state_table.h + * + * Finite-State-Machine helper class for state table + */ + +#ifndef __SYSTEMLIB_STATE_TABLE_H +#define __SYSTEMLIB_STATE_TABLE_H + +class StateTable +{ +public: + typedef void (StateTable::*Action)(); + struct Tran { + Action action; + unsigned nextState; + }; + + StateTable(Tran const *table, unsigned nStates, unsigned nSignals) + : myTable(table), myNsignals(nSignals), myNstates(nStates) {} + + #define NO_ACTION &StateTable::doNothing + #define ACTION(_target) static_cast(_target) + + virtual ~StateTable() {} + + void dispatch(unsigned const sig) { + register Tran const *t = myTable + myState*myNsignals + sig; + (this->*(t->action))(); + + myState = t->nextState; + } + void doNothing() {} +protected: + unsigned myState; +private: + Tran const *myTable; + unsigned myNsignals; + unsigned myNstates; +}; + +#endif \ No newline at end of file From b5fb5f9dbb2d26cea1ab50f005cedff52e992eca Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 16 Dec 2013 16:59:24 +0100 Subject: [PATCH 7/7] Navigator: Moved mission stuff in separate class --- src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator_main.cpp | 343 ++++++++------------ src/modules/navigator/navigator_mission.cpp | 234 +++++++++++++ src/modules/navigator/navigator_mission.h | 95 ++++++ 4 files changed, 458 insertions(+), 217 deletions(-) create mode 100644 src/modules/navigator/navigator_mission.cpp create mode 100644 src/modules/navigator/navigator_mission.h diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 7f7443c43e..fc59c956a3 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ - navigator_params.c + navigator_params.c \ + navigator_mission.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d258aa8a69..d93ecc7cd3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -75,6 +75,7 @@ #include #include +#include "navigator_mission.h" /* oddly, ERROR is not defined for c++ */ @@ -99,7 +100,7 @@ public: Navigator(); /** - * Destructor, also kills the sensors task. + * Destructor, also kills the navigators task. */ ~Navigator(); @@ -158,16 +159,7 @@ private: struct navigation_capabilities_s _nav_caps; - unsigned _current_offboard_mission_index; - unsigned _current_onboard_mission_index; - unsigned _offboard_mission_item_count; /** number of offboard mission items copied */ - unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ - - enum { - MISSION_TYPE_NONE, - MISSION_TYPE_ONBOARD, - MISSION_TYPE_OFFBOARD, - } _mission_type; + class Mission _mission; bool _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -293,17 +285,12 @@ private: /** * Move to next waypoint */ - int advance_mission(); + void advance_mission(); /** - * Helper function to set a mission item + * Helper function to get a loiter item */ - int set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) ; - - /** - * Helper function to set a loiter item - */ - void set_loiter_item(mission_item_s *new_loiter_position); + void get_loiter_item(mission_item_s *new_loiter_position); /** * Publish a new mission item triplet for position controller @@ -319,6 +306,8 @@ private: * @return true if equivalent, false otherwise */ bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b); + + void add_home_pos_to_rtl(struct mission_item_s *new_mission_item); }; namespace navigator @@ -362,10 +351,7 @@ Navigator::Navigator() : /* states */ _fence_valid(false), _inside_fence(true), - _current_offboard_mission_index(0), - _current_onboard_mission_index(0), - _offboard_mission_item_count(0), - _onboard_mission_item_count(0), + _mission(), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0) @@ -424,6 +410,8 @@ 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)); + + _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); } void @@ -452,15 +440,12 @@ Navigator::offboard_mission_update() struct mission_s offboard_mission; if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { - _offboard_mission_item_count = offboard_mission.count; - - if (offboard_mission.current_index != -1) { - _current_offboard_mission_index = offboard_mission.current_index; - } + _mission.set_current_offboard_mission_index(offboard_mission.current_index); + _mission.set_offboard_mission_count(offboard_mission.count); } else { - _offboard_mission_item_count = 0; - _current_offboard_mission_index = 0; + _mission.set_current_offboard_mission_index(0); + _mission.set_offboard_mission_count(0); } } @@ -468,17 +453,14 @@ void Navigator::onboard_mission_update() { struct mission_s onboard_mission; - if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { + if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { - _onboard_mission_item_count = onboard_mission.count; - - if (onboard_mission.current_index != -1) { - _current_onboard_mission_index = onboard_mission.current_index; - } + _mission.set_current_onboard_mission_index(onboard_mission.current_index); + _mission.set_onboard_mission_count(onboard_mission.count); } else { - _onboard_mission_item_count = 0; - _current_onboard_mission_index = 0; + _mission.set_current_onboard_mission_index(0); + _mission.set_onboard_mission_count(0); } } @@ -594,7 +576,7 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_MISSION: /* try mission if none is available, fallback to loiter instead */ - if (onboard_mission_available(0) || offboard_mission_available(0)) { + if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); } else { dispatch(EVENT_LOITER_REQUESTED); @@ -671,8 +653,22 @@ Navigator::task_main() global_position_update(); /* only check if waypoint has been reached in Mission or RTL mode */ if (mission_item_reached()) { - /* try to advance mission */ - if (advance_mission() != OK) { + + if (_vstatus.main_state == MAIN_STATE_AUTO && + (_vstatus.navigation_state == NAVIGATION_STATE_AUTO_READY || + _vstatus.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || + _vstatus.navigation_state == NAVIGATION_STATE_AUTO_MISSION)) { + + /* advance by one mission item */ + _mission.move_to_next(); + + /* if no more mission items available send this to state machine and start loiter at the last WP */ + if (_mission.current_mission_available()) { + advance_mission(); + } else { + dispatch(EVENT_MISSION_FINISHED); + } + } else { dispatch(EVENT_MISSION_FINISHED); } } @@ -740,20 +736,6 @@ Navigator::status() break; case STATE_MISSION: warnx("State: Mission"); - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - warnx("Mission type: Onboard"); - break; - case MISSION_TYPE_OFFBOARD: - warnx("Mission type: Offboard"); - break; - case MISSION_TYPE_NONE: - default: - warnx("ERROR: Mission type unsupported"); - break; - } - warnx("Onboard mission: %d of %d", _current_onboard_mission_index, _onboard_mission_item_count); - warnx("Offboard mission: %d of %d", _current_offboard_mission_index, _offboard_mission_item_count); break; case STATE_MISSION_LOITER: warnx("State: Loiter after Mission"); @@ -946,26 +928,28 @@ Navigator::start_none() void Navigator::start_loiter() { - struct mission_item_s loiter_item; + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - loiter_item.lat = (double)_global_pos.lat / 1e7; - loiter_item.lon = (double)_global_pos.lon / 1e7; - loiter_item.yaw = 0.0f; + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7; + _mission_item_triplet.current.yaw = 0.0f; + + get_loiter_item(&_mission_item_triplet.current); /* XXX get rid of ugly conversion for home position altitude */ float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; /* Use current altitude if above min altitude set by parameter */ if (_global_pos.alt < global_min_alt) { - loiter_item.altitude = global_min_alt; + _mission_item_triplet.current.altitude = global_min_alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); } else { - loiter_item.altitude = _global_pos.alt; + _mission_item_triplet.current.altitude = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); } - set_loiter_item(&loiter_item); - publish_mission_item_triplet(); } @@ -975,86 +959,86 @@ Navigator::start_mission() { /* leave previous mission item as isas is */ - if(set_mission_item(0, &_mission_item_triplet.current) == OK) { + int ret; + bool onboard; + unsigned index; + + ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.current); _mission_item_triplet.current_valid = true; + + if (onboard) { + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + } } else { + /* since a mission is not started without WPs available, this is not supposed to happen */ _mission_item_triplet.current_valid = false; warnx("ERROR: current WP can't be set"); } - if(set_mission_item(1, &_mission_item_triplet.next) == OK) { + ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.next); _mission_item_triplet.next_valid = true; } else { + /* this will fail for the last WP */ _mission_item_triplet.next_valid = false; } - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", _current_onboard_mission_index); - break; - case MISSION_TYPE_OFFBOARD: - mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", _current_offboard_mission_index); - break; - case MISSION_TYPE_NONE: - default: - warnx("ERROR: Mission type unsupported"); - break; - } - publish_mission_item_triplet(); } -int +void Navigator::advance_mission() { - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - warnx("advance onboard before: %d", _current_onboard_mission_index); - _current_onboard_mission_index++; - warnx("advance onboard after: %d", _current_onboard_mission_index); - break; - case MISSION_TYPE_OFFBOARD: - warnx("advance offboard before: %d", _current_offboard_mission_index); - _current_offboard_mission_index++; - warnx("advance offboard after: %d", _current_offboard_mission_index); - break; - case MISSION_TYPE_NONE: - default: - warnx("ERROR: Mission type unsupported"); - return ERROR; - } - - /* if there is no more mission available, don't advance and switch to loiter at current WP */ - if (!_mission_item_triplet.next_valid) { - warnx("no next valid"); - return ERROR; - } - /* 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; - if(set_mission_item(0, &_mission_item_triplet.current) == OK) { + int ret; + bool onboard; + unsigned index; + + ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.current); _mission_item_triplet.current_valid = true; - + + if (onboard) { + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + } } else { - /* should never ever happen */ + /* since a mission is not advanced without WPs available, this is not supposed to happen */ _mission_item_triplet.current_valid = false; - warnx("no current available"); - return ERROR; + warnx("ERROR: current WP can't be set"); } - - if(set_mission_item(1, &_mission_item_triplet.next) == OK) { + + ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.next); + _mission_item_triplet.next_valid = true; - } else { + /* this will fail for the last WP */ _mission_item_triplet.next_valid = false; } publish_mission_item_triplet(); - return OK; } void @@ -1063,23 +1047,13 @@ Navigator::start_mission_loiter() /* make sure the current WP is valid */ if (!_mission_item_triplet.current_valid) { warnx("ERROR: cannot switch to offboard mission loiter"); - return; } - set_loiter_item(&_mission_item_triplet.current); + get_loiter_item(&_mission_item_triplet.current); - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - mavlink_log_info(_mavlink_fd, "[navigator] loiter at onboard WP %d", _current_onboard_mission_index-1); - break; - case MISSION_TYPE_OFFBOARD: - mavlink_log_info(_mavlink_fd, "[navigator] loiter at offboard WP %d", _current_offboard_mission_index-1); - break; - case MISSION_TYPE_NONE: - default: - warnx("ERROR: Mission type unsupported"); - break; - } + publish_mission_item_triplet(); + + mavlink_log_info(_mavlink_fd, "[navigator] loiter at last WP"); } void @@ -1111,85 +1085,21 @@ Navigator::start_rtl() void Navigator::start_rtl_loiter() { - mission_item_s home_position_mission_item; - home_position_mission_item.lat = (double)_home_pos.lat / 1e7; - home_position_mission_item.lon = (double)_home_pos.lon / 1e7; - home_position_mission_item.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; + _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; + _mission_item_triplet.current.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; - set_loiter_item(&home_position_mission_item); + get_loiter_item(&_mission_item_triplet.current); + + publish_mission_item_triplet(); mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); } -bool -Navigator::offboard_mission_available(unsigned relative_index) -{ - return _offboard_mission_item_count > _current_offboard_mission_index + relative_index; -} - -bool -Navigator::onboard_mission_available(unsigned relative_index) -{ - return _onboard_mission_item_count > _current_onboard_mission_index + relative_index && _parameters.onboard_mission_enabled; -} - -int -Navigator::set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) -{ - struct mission_item_s new_mission_item; - - /* try onboard mission first */ - if (onboard_mission_available(relative_index)) { - if (_mission_type != MISSION_TYPE_ONBOARD && relative_index == 1) { - relative_index--; - } - if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + relative_index, &new_mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - _mission_type = MISSION_TYPE_NONE; - return ERROR; - } - /* base the mission type on the current mission item, not on future ones */ - if (relative_index == 0) { - _mission_type = MISSION_TYPE_ONBOARD; - } - /* otherwise fallback to offboard */ - } else if (offboard_mission_available(relative_index)) { - - warnx("fallback try offboard: %d / %d", _current_offboard_mission_index + relative_index, _offboard_mission_item_count); - - if (_mission_type != MISSION_TYPE_OFFBOARD && relative_index == 1) { - relative_index--; - } - - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + relative_index, &new_mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - _mission_type = MISSION_TYPE_NONE; - warnx("failed"); - return ERROR; - } - /* base the mission type on the current mission item, not on future ones */ - if (relative_index == 0) { - _mission_type = MISSION_TYPE_OFFBOARD; - } - } else { - /* happens when no more mission items can be added as a next item */ - return ERROR; - } - - if (new_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* if it is a RTL waypoint, append the home position */ - new_mission_item.lat = (double)_home_pos.lat / 1e7; - new_mission_item.lon = (double)_home_pos.lon / 1e7; - new_mission_item.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; - new_mission_item.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_mission_item.radius = 50.0f; // TODO: get rid of magic number - } - - memcpy(mission_item, &new_mission_item, sizeof(struct mission_item_s)); - - return OK; -} - bool Navigator::mission_item_reached() { @@ -1292,24 +1202,13 @@ Navigator::mission_item_reached() } void -Navigator::set_loiter_item(struct mission_item_s *new_loiter_position) +Navigator::get_loiter_item(struct mission_item_s *new_loiter_position) { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - _mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number - _mission_item_triplet.current.autocontinue = false; - - _mission_item_triplet.current.lat = new_loiter_position->lat; - _mission_item_triplet.current.lon = new_loiter_position->lon; - _mission_item_triplet.current.altitude = new_loiter_position->altitude; - _mission_item_triplet.current.yaw = new_loiter_position->yaw; - - publish_mission_item_triplet(); + new_loiter_position->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + new_loiter_position->loiter_direction = 1; + new_loiter_position->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_loiter_position->radius = 50.0f; // TODO: get rid of magic number + new_loiter_position->autocontinue = false; } void @@ -1400,3 +1299,15 @@ int navigator_main(int argc, char *argv[]) return 0; } +void +Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) +{ + if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* if it is a RTL waypoint, append the home position */ + new_mission_item->lat = (double)_home_pos.lat / 1e7; + new_mission_item->lon = (double)_home_pos.lon / 1e7; + new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_mission_item->radius = 50.0f; // TODO: get rid of magic number + } +} \ No newline at end of file diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp new file mode 100644 index 0000000000..993f8f1331 --- /dev/null +++ b/src/modules/navigator/navigator_mission.cpp @@ -0,0 +1,234 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mission.cpp + * Helper class to access missions + */ + +// #include +// #include +// #include +// #include + +#include +#include +#include "navigator_mission.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + + +Mission::Mission() : + + _current_offboard_mission_index(0), + _current_onboard_mission_index(0), + _offboard_mission_item_count(0), + _onboard_mission_item_count(0), + _onboard_mission_allowed(false), + _current_mission_type(MISSION_TYPE_NONE) +{} + +Mission::~Mission() +{ + +} + +void +Mission::set_current_offboard_mission_index(int new_index) +{ + if (new_index != -1) { + _current_offboard_mission_index = (unsigned)new_index; + } +} + +void +Mission::set_current_onboard_mission_index(int new_index) +{ + if (new_index != -1) { + _current_onboard_mission_index = (unsigned)new_index; + } +} + +void +Mission::set_offboard_mission_count(unsigned new_count) +{ + _offboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_count(unsigned new_count) +{ + _onboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_allowed(bool allowed) +{ + _onboard_mission_allowed = allowed; +} + +bool +Mission::current_mission_available() +{ + return (current_onboard_mission_available() || current_offboard_mission_available()); + +} + +bool +Mission::next_mission_available() +{ + return (next_onboard_mission_available() || next_offboard_mission_available()); +} + +int +Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index) +{ + /* try onboard mission first */ + if (current_onboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + _current_mission_type = MISSION_TYPE_ONBOARD; + *onboard = true; + *index = _current_onboard_mission_index; + + /* otherwise fallback to offboard */ + } else if (current_offboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + _current_mission_type = MISSION_TYPE_NONE; + return ERROR; + } + _current_mission_type = MISSION_TYPE_OFFBOARD; + *onboard = false; + *index = _current_offboard_mission_index; + + } else { + /* happens when no more mission items can be added as a next item */ + _current_mission_type = MISSION_TYPE_NONE; + return ERROR; + } + + return OK; +} + +int +Mission::get_next_mission_item(struct mission_item_s *new_mission_item) +{ + /* try onboard mission first */ + if (next_onboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + + /* otherwise fallback to offboard */ + } else if (next_offboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + 1, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + + } else { + /* happens when no more mission items can be added as a next item */ + return ERROR; + } + + return OK; +} + + +bool +Mission::current_onboard_mission_available() +{ + return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed; +} + +bool +Mission::current_offboard_mission_available() +{ + return _offboard_mission_item_count > _current_offboard_mission_index; +} + +bool +Mission::next_onboard_mission_available() +{ + unsigned next = 0; + + if (_current_mission_type != MISSION_TYPE_ONBOARD) { + next = 1; + } + + return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed; +} + +bool +Mission::next_offboard_mission_available() +{ + unsigned next = 0; + + if (_current_mission_type != MISSION_TYPE_OFFBOARD) { + next = 1; + } + + return _offboard_mission_item_count > (_current_offboard_mission_index + next); +} + +void +Mission::move_to_next() +{ + switch (_current_mission_type) { + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + case MISSION_TYPE_NONE: + default: + break; + } +} \ No newline at end of file diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h new file mode 100644 index 0000000000..e8e4763825 --- /dev/null +++ b/src/modules/navigator/navigator_mission.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mission.h + * Helper class to access missions + */ + +#ifndef NAVIGATOR_MISSION_H +#define NAVIGATOR_MISSION_H + +#include + + +class __EXPORT Mission +{ +public: + /** + * Constructor + */ + Mission(); + + /** + * Destructor, also kills the sensors task. + */ + ~Mission(); + + void set_current_offboard_mission_index(int new_index); + void set_current_onboard_mission_index(int new_index); + void set_offboard_mission_count(unsigned new_count); + void set_onboard_mission_count(unsigned new_count); + + void set_onboard_mission_allowed(bool allowed); + + bool current_mission_available(); + bool next_mission_available(); + + int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index); + int get_next_mission_item(struct mission_item_s *mission_item); + + void move_to_next(); + + void add_home_pos(struct mission_item_s *new_mission_item); + +private: + bool current_onboard_mission_available(); + bool current_offboard_mission_available(); + bool next_onboard_mission_available(); + bool next_offboard_mission_available(); + + unsigned _current_offboard_mission_index; + unsigned _current_onboard_mission_index; + unsigned _offboard_mission_item_count; /** number of offboard mission items available */ + unsigned _onboard_mission_item_count; /** number of onboard mission items available */ + + bool _onboard_mission_allowed; + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_ONBOARD, + MISSION_TYPE_OFFBOARD, + } _current_mission_type; +}; + +#endif \ No newline at end of file