diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 1e86b1b6ca..fbfd1259dd 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -225,29 +225,27 @@ Mission::set_mission_items() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - set_previous_pos_setpoint(pos_sp_triplet); + /* set previous position setpoint to current */ + set_previous_pos_setpoint(); /* try setting onboard mission item */ - if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { + if (set_mission_item(true, false, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: onboard mission running"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running"); } _mission_type = MISSION_TYPE_ONBOARD; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); /* try setting offboard mission item */ - } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { + } else if (set_mission_item(false, false, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: offboard mission running"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); } _mission_type = MISSION_TYPE_OFFBOARD; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); } else { + /* no mission available, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: mission finished"); @@ -257,37 +255,92 @@ Mission::set_mission_items() } _mission_type = MISSION_TYPE_NONE; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); - + /* set loiter mission item */ set_loiter_item(&_mission_item); + /* update position setpoint triplet */ pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + reset_mission_item_reached(); report_mission_finished(); + + _navigator->set_position_setpoint_triplet_updated(); + return; } + + /* new current mission item set */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); + + if (_mission_type == MISSION_TYPE_OFFBOARD) { + report_current_offboard_mission_item(); + } + // TODO: report onboard mission item somehow + + /* 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)) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } + + _navigator->set_position_setpoint_triplet_updated(); } bool -Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) +Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *mission_item) { /* make sure param is up to date */ updateParams(); - if (_param_onboard_enabled.get() > 0 && - _current_onboard_mission_index >= 0&& - _current_onboard_mission_index < (int)_onboard_mission.count) { + + /* select onboard/offboard mission */ + int *mission_index_ptr; + struct mission_s *mission; + dm_item_t dm_item; + int mission_index_next; + + 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 = &_onboard_mission; + + dm_item = DM_KEY_WAYPOINTS_ONBOARD; + + } else { + /* offboard mission */ + mission_index_next = _current_offboard_mission_index + 1; + mission_index_ptr = next_item ? &mission_index_next : &_current_offboard_mission_index; + + mission = &_offboard_mission; + + if (_offboard_mission.dataman_id == 0) { + dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0; + + } else { + dm_item = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + } + + if (*mission_index_ptr >= 0 && *mission_index_ptr < (int)mission->count) { struct mission_item_s new_mission_item; - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, - &new_mission_item)) { - /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); - reset_mission_item_reached(); - - /* TODO: report this somehow */ - memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); + 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; } } @@ -295,88 +348,15 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current } bool -Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) -{ - if (_current_offboard_mission_index >= 0 && - _current_offboard_mission_index < (int)_offboard_mission.count) { - dm_item_t dm_current; - if (_offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - struct mission_item_s new_mission_item; - if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) { - /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); - - reset_mission_item_reached(); - - report_current_offboard_mission_item(); - memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); - return true; - } - } - return false; -} - -void -Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) -{ - int next_temp_mission_index = _onboard_mission.current_index + 1; - - /* try if there is a next onboard mission */ - if (_onboard_mission.current_index >= 0 && - next_temp_mission_index < (int)_onboard_mission.count) { - struct mission_item_s new_mission_item; - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { - /* convert next mission item to position setpoint */ - mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - return; - } - } - - /* give up */ - next_pos_sp->valid = false; - return; -} - -void -Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) -{ - /* try if there is a next offboard mission */ - int next_temp_mission_index = _offboard_mission.current_index + 1; - warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); - if (_offboard_mission.current_index >= 0 && - next_temp_mission_index < (int)_offboard_mission.count) { - dm_item_t dm_current; - if (_offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - struct mission_item_s new_mission_item; - if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) { - /* convert next mission item to position setpoint */ - mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - return; - } - } - /* give up */ - next_pos_sp->valid = false; - return; -} - -bool -Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, +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++) { + for (int i = 0; i < 10; i++) { const ssize_t len = sizeof(struct mission_item_s); /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { + if (dm_read(dm_item, *mission_index_ptr, new_mission_item, 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"); @@ -394,7 +374,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio if (is_current) { (new_mission_item->do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ @@ -405,12 +385,12 @@ 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 = new_mission_item->do_jump_mission_index; + *mission_index_ptr = new_mission_item->do_jump_mission_index; } else { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: DO JUMP repetitions completed"); /* no more DO_JUMPS, therefore just try to continue with next mission item */ - (*mission_index)++; + (*mission_index_ptr)++; } } else { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index ac293990f7..7d07860ded 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -97,16 +97,10 @@ private: void set_mission_items(); /** - * Try to set the current position setpoint from an onboard mission item + * Try to set given mission item from an offboard/onboard mission item * @return true if mission item successfully set */ - bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp); - - /** - * Try to set the current position setpoint from an offboard mission item - * @return true if mission item successfully set - */ - bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp); + 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 diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index bf3c934a6b..cbf7ac9871 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -202,9 +202,10 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite } void -MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet) +MissionBlock::set_previous_pos_setpoint() { - /* reuse current setpoint as previous setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + if (pos_sp_triplet->current.valid) { memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s)); } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 22f42cc318..adf50bc391 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -86,7 +86,7 @@ protected: /** * Set previous position setpoint to current setpoint */ - void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet); + void set_previous_pos_setpoint(); /** * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 7cf6bb1a8b..6d7afcf4b5 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -82,7 +82,7 @@ RTL::on_inactive() } void -RTL::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +RTL::on_activation() { /* decide where to enter the RTL procedure when we switch into it */ if (_rtl_state == RTL_STATE_NONE) { @@ -105,28 +105,27 @@ RTL::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) } } - set_rtl_item(pos_sp_triplet); -} - -bool -RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) -{ - if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { - advance_rtl(); - set_rtl_item(pos_sp_triplet); - return true; - } - - return false; + set_rtl_item(); } void -RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) +RTL::on_active() { + if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { + advance_rtl(); + set_rtl_item(); + } +} + +void +RTL::set_rtl_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + /* make sure we have the latest params */ updateParams(); - set_previous_pos_setpoint(pos_sp_triplet); + set_previous_pos_setpoint(); _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { @@ -273,11 +272,13 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) break; } + reset_mission_item_reached(); + /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - reset_mission_item_reached(); - pos_sp_triplet->current.valid = true; pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); } void diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 9c8b3fdfc3..5928f1f07b 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -63,15 +63,15 @@ public: virtual void on_inactive(); - virtual void on_activation(position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); - virtual bool on_active(position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_active(); private: /** * Set the RTL item */ - void set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet); + void set_rtl_item(); /** * Move to next RTL item