From a4315aee2ee69068c9a13568091805e20d0b04c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Jun 2014 10:47:08 +0200 Subject: navigator: mission mode fixed --- src/modules/navigator/mission.cpp | 63 ++++++++++++++++----------------------- src/modules/navigator/mission.h | 21 ++----------- 2 files changed, 27 insertions(+), 57 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index fbfd1259d..f207776a9 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -70,9 +70,9 @@ Mission::Mission(Navigator *navigator, const char *name) : { /* load initial params */ updateParams(); + /* set initial mission items */ on_inactive(); - } Mission::~Mission() @@ -121,16 +121,12 @@ Mission::on_active() /* reset mission items if needed */ if (onboard_updated || offboard_updated) { set_mission_items(); - - _navigator->set_position_setpoint_triplet_updated(); } /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { advance_mission(); set_mission_items(); - - _navigator->set_position_setpoint_triplet_updated(); } } @@ -223,13 +219,16 @@ Mission::advance_mission() void Mission::set_mission_items() { + /* make sure param is up to date */ + updateParams(); + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* set previous position setpoint to current */ set_previous_pos_setpoint(); /* try setting onboard mission item */ - if (set_mission_item(true, false, &_mission_item)) { + if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running"); @@ -237,7 +236,7 @@ Mission::set_mission_items() _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (set_mission_item(false, false, &_mission_item)) { + } else if (read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); @@ -284,7 +283,8 @@ Mission::set_mission_items() /* try to read next mission item */ struct mission_item_s mission_item_next; - if (!set_mission_item(_mission_type == MISSION_TYPE_ONBOARD, true, &mission_item_next)) { + + if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { /* got next mission item, update setpoint triplet */ mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); @@ -297,11 +297,8 @@ Mission::set_mission_items() } bool -Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *mission_item) +Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item) { - /* make sure param is up to date */ - updateParams(); - /* select onboard/offboard mission */ int *mission_index_ptr; struct mission_s *mission; @@ -310,12 +307,8 @@ Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *m if (onboard) { /* onboard mission */ - if (!_param_onboard_enabled.get()) { - return false; - } - mission_index_next = _current_onboard_mission_index + 1; - mission_index_ptr = next_item ? &mission_index_next : &_current_onboard_mission_index; + mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next; mission = &_onboard_mission; @@ -324,7 +317,7 @@ Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *m } else { /* offboard mission */ mission_index_next = _current_offboard_mission_index + 1; - mission_index_ptr = next_item ? &mission_index_next : &_current_offboard_mission_index; + mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next; mission = &_offboard_mission; @@ -336,27 +329,20 @@ Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *m } } - if (*mission_index_ptr >= 0 && *mission_index_ptr < (int)mission->count) { - struct mission_item_s new_mission_item; - - if (read_mission_item(dm_item, true, mission_index_ptr, &new_mission_item)) { - memcpy(&mission_item, &new_mission_item, sizeof(struct mission_item_s)); - return true; - } + if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { + /* mission item index out of bounds */ + return false; } - return false; -} -bool -Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index_ptr, - struct mission_item_s *new_mission_item) -{ /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ for (int i = 0; i < 10; i++) { const ssize_t len = sizeof(struct mission_item_s); + /* read mission item to temp storage first to not overwrite current mission item if data damaged */ + struct mission_item_s mission_item_tmp; + /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index_ptr, new_mission_item, len) != len) { + if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_log_critical(_navigator->get_mavlink_fd(), "#audio: ERROR waypoint could not be read"); @@ -364,18 +350,17 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ - if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { + if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) { /* do DO_JUMP as many times as requested */ - if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) { /* only raise the repeat count if this is for the current mission item * but not for the next mission item */ if (is_current) { - (new_mission_item->do_jump_current_count)++; + (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, - new_mission_item, len) != len) { + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), @@ -385,7 +370,8 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index_ptr = new_mission_item->do_jump_mission_index; + *mission_index_ptr = mission_item_tmp.do_jump_mission_index; + } else { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: DO JUMP repetitions completed"); @@ -395,6 +381,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } else { /* if it's not a DO_JUMP, then we were successful */ + memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s)); return true; } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 7d07860de..29e4d41f6 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -97,27 +97,10 @@ private: void set_mission_items(); /** - * Try to set given mission item from an offboard/onboard mission item - * @return true if mission item successfully set - */ - bool set_mission_item(bool onboard, bool next_item, struct mission_item_s *mission_item); - - /** - * Try to set the next position setpoint from an onboard mission item - */ - void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp); - - /** - * Try to set the next position setpoint from an offboard mission item - */ - void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp); - - /** - * Read a mission item from the dataman and watch out for DO_JUMPS + * Read current or next mission item from the dataman and watch out for DO_JUMPS * @return true if successful */ - bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, - struct mission_item_s *new_mission_item); + bool read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item); /** * Report that a mission item has been reached -- cgit v1.2.3