diff options
author | Julian Oes <julian@oes.ch> | 2013-12-25 11:23:58 +0100 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-12-25 11:23:58 +0100 |
commit | b10fa3d0476ce6af977fe7e54bc361c44c27e9c4 (patch) | |
tree | 7c230c829f4a43d1f12d8936c4a67d170abedc3a /src/modules/navigator | |
parent | 5c33aeeb430a984d0802f1af73063afca793a98a (diff) | |
download | px4-firmware-b10fa3d0476ce6af977fe7e54bc361c44c27e9c4.tar.gz px4-firmware-b10fa3d0476ce6af977fe7e54bc361c44c27e9c4.tar.bz2 px4-firmware-b10fa3d0476ce6af977fe7e54bc361c44c27e9c4.zip |
Waypoints/Navigator: Use two different dataman storage places, keep old waypoints until all new ones are written
Diffstat (limited to 'src/modules/navigator')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 1 | ||||
-rw-r--r-- | src/modules/navigator/navigator_mission.cpp | 29 | ||||
-rw-r--r-- | src/modules/navigator/navigator_mission.h | 2 |
3 files changed, 29 insertions, 3 deletions
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), @@ -68,6 +69,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) { if (new_index != -1) { @@ -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 */ |