From b10fa3d0476ce6af977fe7e54bc361c44c27e9c4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 25 Dec 2013 11:23:58 +0100 Subject: Waypoints/Navigator: Use two different dataman storage places, keep old waypoints until all new ones are written --- src/modules/dataman/dataman.c | 3 ++- src/modules/dataman/dataman.h | 6 +++-- src/modules/mavlink/waypoints.c | 34 +++++++++++++++++++++++------ src/modules/mavlink/waypoints.h | 1 + src/modules/navigator/navigator_main.cpp | 1 + src/modules/navigator/navigator_mission.cpp | 29 +++++++++++++++++++++--- src/modules/navigator/navigator_mission.h | 2 ++ src/modules/uORB/topics/mission.h | 1 + 8 files changed, 64 insertions(+), 13 deletions(-) (limited to 'src/modules') diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 14112fc0d..dc2d6c312 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -111,7 +111,8 @@ 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_OFFBOARD_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, DM_KEY_WAYPOINTS_ONBOARD_MAX }; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 2a781405a..a70638ccc 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_OFFBOARD, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ 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 +60,8 @@ extern "C" { enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - DM_KEY_WAYPOINTS_OFFBOARD_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_OFFBOARD_1_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 0998cd5eb..45e891434 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -169,6 +169,8 @@ void mavlink_wpm_init(mavlink_wpm_storage *state) state->timestamp_lastaction = 0; // state->timestamp_last_send_setpoint = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + + state->current_dataman_id = 0; // state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; // state->idle = false; ///< indicates if the system is following the waypoints or is waiting // state->current_active_wp_id = -1; ///< id of current waypoint @@ -612,6 +614,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // wpm->yaw_reached = false; // wpm->pos_reached = false; + mission.current_index = wpc.seq; publish_mission(); @@ -718,7 +721,15 @@ 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_OFFBOARD, wpr.seq, &mission_item, len) == len) { + dm_item_t dm_current; + + if (wpm->current_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + + if (dm_read(dm_current, wpr.seq, &mission_item, len) == len) { if (mission.current_index == wpr.seq) { wp.current = true; @@ -855,10 +866,6 @@ 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 { - /* set count to 0 while copying */ - mission.count = 0; - publish_mission(); } #ifdef MAVLINK_WPM_NO_PRINTF @@ -992,7 +999,17 @@ 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_OFFBOARD, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + dm_item_t dm_next; + + if (wpm->current_dataman_id == 0) { + dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; + mission.dataman_id = 1; + } else { + dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.dataman_id = 0; + } + + if (dm_write(dm_next, 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; @@ -1038,6 +1055,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi publish_mission(); + wpm->current_dataman_id = mission.dataman_id; wpm->size = wpm->current_count; //get the new current waypoint @@ -1132,9 +1150,11 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // wpm->pos_reached = false; /* prepare mission topic */ + mission.dataman_id = -1; mission.count = 0; + mission.current_index = -1; - if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD) == OK) { + if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == 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/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index fc68285e9..801bc0bcf 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -110,6 +110,7 @@ struct mavlink_wpm_storage { // uint64_t timestamp_firstinside_orbit; // uint64_t timestamp_lastoutside_orbit; uint32_t timeout; + int current_dataman_id; // uint32_t delay_setpoint; // float accept_range_yaw; // float accept_range_distance; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d93ecc7cd..48f828ff7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -440,6 +440,7 @@ Navigator::offboard_mission_update() struct mission_s offboard_mission; if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + _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); diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index 993f8f133..6576aae70 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -53,7 +53,8 @@ static const int ERROR = -1; Mission::Mission() : - + + _offboard_dataman_id(-1), _current_offboard_mission_index(0), _current_onboard_mission_index(0), _offboard_mission_item_count(0), @@ -67,6 +68,12 @@ Mission::~Mission() } +void +Mission::set_offboard_dataman_id(int new_id) +{ + _offboard_dataman_id = new_id; +} + void Mission::set_current_offboard_mission_index(int new_index) { @@ -132,8 +139,16 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool /* otherwise fallback to offboard */ } else if (current_offboard_mission_available()) { + dm_item_t dm_current; + + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + 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) { + if (dm_read(dm_current, _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; @@ -166,8 +181,16 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item) /* otherwise fallback to offboard */ } else if (next_offboard_mission_available()) { + dm_item_t dm_current; + + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + 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) { + if (dm_read(dm_current, _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; } diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index e8e476382..15d4e86bf 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -55,6 +55,7 @@ public: */ ~Mission(); + void set_offboard_dataman_id(int new_id); 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); @@ -78,6 +79,7 @@ private: bool next_onboard_mission_available(); bool next_offboard_mission_available(); + int _offboard_dataman_id; unsigned _current_offboard_mission_index; unsigned _current_onboard_mission_index; unsigned _offboard_mission_item_count; /** number of offboard mission items available */ diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 9697835cf..6d4b50a9b 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -97,6 +97,7 @@ struct mission_item_s struct mission_s { + int dataman_id; /**< default -1, there are two offboard storage places in the dataman: 0 or 1 */ unsigned count; /**< count of the missions stored in the datamanager */ int current_index; /**< default -1, start at the one changed latest */ }; -- cgit v1.2.3