fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 19:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Waypoints/Navigator: Use two different dataman storage places, keep old waypoints until all new ones are written

Browse Source
This commit is contained in:
Julian Oes
2013-12-25 11:23:58 +01:00
parent 5c33aeeb43
commit b10fa3d047
8 changed files with 64 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+1
View File
@@ -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);
src/modules/navigator/navigator_mission.cpp
+26 -3
View File
@@ -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;
}
src/modules/navigator/navigator_mission.h
+2
View File
@@ -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 */