From 30dacbd1541073f9e35b8de0d1410e23f3aaccda Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 15 Feb 2014 11:29:36 +0100 Subject: Mavlink and navigator: handle current waypoint onboard --- src/modules/mavlink/mavlink_main.cpp | 102 +++++++++++++++------------- src/modules/mavlink/mavlink_main.h | 2 +- src/modules/navigator/navigator_main.cpp | 25 ++++--- src/modules/navigator/navigator_mission.cpp | 67 +++++++++++++++--- src/modules/navigator/navigator_mission.h | 11 ++- src/modules/uORB/topics/mission_result.h | 3 +- 6 files changed, 142 insertions(+), 68 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d2c94ca92..38f753c39 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -59,11 +59,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -958,7 +958,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa); mavlink_missionlib_send_message(&msg); - if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); + if (_verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } /* @@ -981,11 +981,11 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); - if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); + if (_verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); } else { mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - if (verbose) warnx("ERROR: index out of bounds"); + if (_verbose) warnx("ERROR: index out of bounds"); } } @@ -1001,7 +1001,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); - if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); + if (_verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) @@ -1031,10 +1031,10 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp); mavlink_missionlib_send_message(&msg); - if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); + if (_verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - if (verbose) warnx("ERROR: could not read WP%u", seq); + if (_verbose) warnx("ERROR: could not read WP%u", seq); } } @@ -1049,11 +1049,11 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr); mavlink_missionlib_send_message(&msg); - if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); + if (_verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } else { mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - if (verbose) warnx("ERROR: Waypoint index exceeds list capacity"); + if (_verbose) warnx("ERROR: Waypoint index exceeds list capacity"); } } @@ -1074,10 +1074,9 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); - if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); + if (_verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); } - void Mavlink::mavlink_waypoint_eventloop(uint64_t now) { /* check for timed-out operations */ @@ -1085,7 +1084,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) mavlink_missionlib_send_gcs_string("Operation timeout"); - if (verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); + if (_verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); wpm->current_state = MAVLINK_WPM_STATE_IDLE; wpm->current_partner_sysid = 0; @@ -1117,7 +1116,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - if (verbose) warnx("REJ. WP CMD: curr partner id mismatch"); + if (_verbose) warnx("REJ. WP CMD: curr partner id mismatch"); } break; @@ -1140,18 +1139,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - if (verbose) warnx("IGN WP CURR CMD: Not in list"); + if (_verbose) warnx("IGN WP CURR CMD: Not in list"); } } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - if (verbose) warnx("IGN WP CURR CMD: Busy"); + if (_verbose) warnx("IGN WP CURR CMD: Busy"); } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (verbose) warnx("REJ. WP CMD: target id mismatch"); + if (_verbose) warnx("REJ. WP CMD: target id mismatch"); } break; @@ -1173,7 +1172,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) wpm->current_partner_compid = msg->compid; } else { - if (verbose) warnx("No waypoints send"); + if (_verbose) warnx("No waypoints send"); } wpm->current_count = wpm->size; @@ -1181,11 +1180,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - if (verbose) warnx("IGN REQUEST LIST: Busy"); + if (_verbose) warnx("IGN REQUEST LIST: Busy"); } } else { mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); - if (verbose) warnx("REJ. REQUEST LIST: target id mismatch"); + if (_verbose) warnx("REJ. REQUEST LIST: target id mismatch"); } break; @@ -1201,7 +1200,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq >= wpm->size) { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); + if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); break; } @@ -1212,11 +1211,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpr.seq == 0) { - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - if (verbose) warnx("REJ. WP CMD: First id != 0"); + if (_verbose) warnx("REJ. WP CMD: First id != 0"); break; } @@ -1224,22 +1223,22 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq == wpm->current_wp_id) { - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } else if (wpr.seq == wpm->current_wp_id + 1) { - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); + if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); break; } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); + if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); break; } @@ -1252,7 +1251,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - if (verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq); + if (_verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } @@ -1261,12 +1260,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); + if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1282,18 +1281,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); + if (_verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE); break; } if (wpc.count == 0) { mavlink_missionlib_send_gcs_string("COUNT 0"); - if (verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); + if (_verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); break; } - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); + if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); wpm->current_state = MAVLINK_WPM_STATE_GETLIST; wpm->current_wp_id = 0; @@ -1307,19 +1306,19 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_wp_id == 0) { mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); + if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); + if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); } } else { mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - if (verbose) warnx("IGN MISSION_COUNT CMD: Busy"); + if (_verbose) warnx("IGN MISSION_COUNT CMD: Busy"); } } else { mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + if (_verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1389,15 +1388,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } - if (wp.current) { - mission.current_index = wp.seq; - } +// if (wp.current) { +// warnx("current is: %d", wp.seq); +// mission.current_index = wp.seq; +// } + // XXX ignore current set + mission.current_index = -1; wpm->current_wp_id = wp.seq + 1; if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); + if (_verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); @@ -1416,7 +1418,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } break; @@ -1448,14 +1450,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - if (verbose) warnx("IGN WP CLEAR CMD: Busy"); + if (_verbose) warnx("IGN WP CLEAR CMD: Busy"); } } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + if (_verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } break; @@ -1550,6 +1552,9 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MODE_ONBOARD; break; + case 'v': + _verbose = true; + default: usage(); break; @@ -1767,9 +1772,12 @@ Mavlink::task_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); - if (mission_result.mission_reached) { - mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index); - } + if (_verbose) warnx("Got mission result"); + + if (mission_result.mission_reached) + mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); + + mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); } mavlink_waypoint_eventloop(hrt_absolute_time()); @@ -1842,7 +1850,7 @@ Mavlink::status() static void usage() { - errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o]"); + errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o] [-v]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c667a41da..9461a51f8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -293,7 +293,7 @@ private: mavlink_wpm_storage wpm_s; mavlink_wpm_storage *wpm; - bool verbose; + bool _verbose; int _uart; int _baudrate; bool gcs_link; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 260356eca..e1dde35bf 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -65,7 +65,6 @@ #include #include #include -#include #include #include #include @@ -288,9 +287,9 @@ private: static void task_main_trampoline(int argc, char *argv[]); /** - * Main sensor collection task. + * Main task. */ - void task_main() __attribute__((noreturn)); + void task_main(); void publish_safepoints(unsigned points); @@ -384,7 +383,6 @@ Navigator::Navigator() : /* publications */ _pos_sp_triplet_pub(-1), - _mission_result_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -416,7 +414,6 @@ Navigator::Navigator() : _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); - memset(&_mission_result, 0, sizeof(struct mission_result_s)); memset(&_mission_item, 0, sizeof(struct mission_item_s)); memset(&nav_states_str, 0, sizeof(nav_states_str)); @@ -518,13 +515,16 @@ Navigator::offboard_mission_update(bool isrotaryWing) missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); - _mission.set_current_offboard_mission_index(offboard_mission.current_index); + _mission.set_offboard_mission_count(offboard_mission.count); + _mission.set_current_offboard_mission_index(offboard_mission.current_index); } else { - _mission.set_current_offboard_mission_index(0); _mission.set_offboard_mission_count(0); + _mission.set_current_offboard_mission_index(0); } + + _mission.publish_mission_result(); } void @@ -534,12 +534,12 @@ Navigator::onboard_mission_update() if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { - _mission.set_current_onboard_mission_index(onboard_mission.current_index); _mission.set_onboard_mission_count(onboard_mission.count); + _mission.set_current_onboard_mission_index(onboard_mission.current_index); } else { - _mission.set_current_onboard_mission_index(0); _mission.set_onboard_mission_count(0); + _mission.set_current_onboard_mission_index(0); } } @@ -1116,6 +1116,9 @@ Navigator::set_mission_item() ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { + + _mission.report_current_mission_item(); + /* reset time counter for new item */ _time_first_inside_orbit = 0; @@ -1537,6 +1540,9 @@ void Navigator::on_mission_item_reached() { if (myState == NAV_STATE_MISSION) { + + _mission.report_mission_item_reached(); + if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; @@ -1589,6 +1595,7 @@ Navigator::on_mission_item_reached() mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); dispatch(EVENT_READY_REQUESTED); } + _mission.publish_mission_result(); } void diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index e72caf98e..fdc4ffff6 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -36,13 +36,12 @@ * Helper class to access missions */ -// #include -// #include -// #include -// #include - +#include #include #include +#include +#include +#include #include "navigator_mission.h" /* oddly, ERROR is not defined for c++ */ @@ -60,8 +59,11 @@ Mission::Mission() : _offboard_mission_item_count(0), _onboard_mission_item_count(0), _onboard_mission_allowed(false), - _current_mission_type(MISSION_TYPE_NONE) -{} + _current_mission_type(MISSION_TYPE_NONE), + _mission_result_pub(-1) +{ + memset(&_mission_result, 0, sizeof(struct mission_result_s)); +} Mission::~Mission() { @@ -78,8 +80,16 @@ void Mission::set_current_offboard_mission_index(int new_index) { if (new_index != -1) { + warnx("specifically set to %d", new_index); _current_offboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_offboard_mission_index >= _offboard_mission_item_count) { + _current_offboard_mission_index = 0; + } } + report_current_mission_item(); } void @@ -87,7 +97,15 @@ Mission::set_current_onboard_mission_index(int new_index) { if (new_index != -1) { _current_onboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_onboard_mission_index >= _onboard_mission_item_count) { + _current_onboard_mission_index = 0; + } } + // TODO: implement this for onboard missions as well + // report_current_mission_item(); } void @@ -266,4 +284,37 @@ Mission::move_to_next() default: break; } -} \ No newline at end of file +} + +void +Mission::report_mission_item_reached() +{ + if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + _mission_result.mission_reached = true; + _mission_result.mission_index_reached = _current_offboard_mission_index; + } +} + +void +Mission::report_current_mission_item() +{ + if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + _mission_result.index_current_mission = _current_offboard_mission_index; + } +} + +void +Mission::publish_mission_result() +{ + /* lazily publish the mission result only once available */ + if (_mission_result_pub > 0) { + /* publish 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); + } + /* reset reached bool */ + _mission_result.mission_reached = false; +} diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index 15d4e86bf..845c16583 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -40,6 +40,7 @@ #define NAVIGATOR_MISSION_H #include +#include class __EXPORT Mission @@ -71,7 +72,9 @@ public: void move_to_next(); - void add_home_pos(struct mission_item_s *new_mission_item); + void report_mission_item_reached(); + void report_current_mission_item(); + void publish_mission_result(); private: bool current_onboard_mission_available(); @@ -92,6 +95,10 @@ private: MISSION_TYPE_ONBOARD, MISSION_TYPE_OFFBOARD, } _current_mission_type; + + int _mission_result_pub; + + struct mission_result_s _mission_result; }; -#endif \ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index c99706b97..7c3921198 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -54,7 +54,8 @@ struct mission_result_s { bool mission_reached; /**< true if mission has been reached */ - unsigned mission_index; /**< index of the mission which has been reached */ + unsigned mission_index_reached; /**< index of the mission which has been reached */ + unsigned index_current_mission; /**< index of the current mission */ }; /** -- cgit v1.2.3