From ee254be845f16531990f4574ca7505ed359c4a61 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 11:16:27 +0200 Subject: [PATCH 1/5] navigator: skip takeoff if already above takeoff altitude --- src/modules/navigator/mission.cpp | 75 ++++++++++++++++++------------- src/modules/navigator/mission.h | 4 +- 2 files changed, 45 insertions(+), 34 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c0e37a3eda..98a4340e1c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -107,6 +107,7 @@ Mission::on_inactive() update_offboard_mission(); } + /* require takeoff after non-loiter or landing */ if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) { _need_takeoff = true; } @@ -399,7 +400,7 @@ Mission::set_mission_items() takeoff_alt += _navigator->get_home_position()->alt; } - /* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ + /* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ if (_navigator->get_vstatus()->condition_landed) { takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); @@ -407,45 +408,55 @@ Mission::set_mission_items() takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); + /* check if we already above takeoff altitude */ + if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { + mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - } else { - /* set current position setpoint from mission item */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - - /* require takeoff after landing or idle */ - if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { - _need_takeoff = true; - } - - _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 (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); + _navigator->set_position_setpoint_triplet_updated(); + return; } else { - /* next mission item is not available */ - pos_sp_triplet->next.valid = false; + /* skip takeoff */ + _takeoff = false; } } + /* set current position setpoint from mission item */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + + /* require takeoff after landing or idle */ + if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { + _need_takeoff = true; + } + + _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 (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); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } + _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4da6a11553..1edaa5c8ac 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -142,8 +142,8 @@ private: int _current_onboard_mission_index; int _current_offboard_mission_index; - bool _need_takeoff; - bool _takeoff; + bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ + bool _takeoff; /**< takeoff state flag */ orb_advert_t _mission_result_pub; struct mission_result_s _mission_result; From 7b4448510436ddb3d5c548a4039bfa53c3aabfc4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 18:46:42 +0200 Subject: [PATCH 2/5] mission: takeoff and next waypoint related fixes --- src/modules/navigator/mission.cpp | 35 +++++++++++++++++++++---------- 1 file changed, 24 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 98a4340e1c..128dab7d77 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -344,6 +344,10 @@ Mission::set_mission_items() /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + + /* use last setpoint for loiter */ + _navigator->set_can_loiter_at_sp(true); + } else if (!user_feedback_done) { /* only tell users that we got no mission if there has not been any * better, more specific feedback yet @@ -360,6 +364,7 @@ Mission::set_mission_items() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + /* reuse setpoint for LOITER only if it's not IDLE */ _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); reset_mission_item_reached(); @@ -412,11 +417,13 @@ Mission::set_mission_items() if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); @@ -445,15 +452,21 @@ Mission::set_mission_items() } // TODO: report onboard mission item somehow - /* try to read next mission item */ - struct mission_item_s mission_item_next; + if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) { + /* try to read next mission item */ + struct mission_item_s 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); + 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); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } } else { - /* next mission item is not available */ + /* vehicle will be paused on current waypoint, don't set next item */ pos_sp_triplet->next.valid = false; } From cf4604f5c3614bc5cd1b82dfee693cbac36181ab Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Sep 2014 13:34:44 +0400 Subject: [PATCH 3/5] navigator/mission.cpp: indentation fixed --- src/modules/navigator/mission.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 128dab7d77..719301a0ac 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -417,13 +417,13 @@ Mission::set_mission_items() if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); From 70e5d4027a3b1465d5128dbf9a04cbb6545e043d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 30 Sep 2014 09:08:31 +0400 Subject: [PATCH 4/5] navigator: autocontinue fix --- src/modules/navigator/mission.cpp | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 1040651320..8bacaa4254 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -150,8 +150,19 @@ Mission::on_active() /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { - advance_mission(); - set_mission_items(); + if (_mission_item.autocontinue) { + /* switch to next waypoint if 'autocontinue' flag set */ + advance_mission(); + set_mission_items(); + + } else { + /* else just report that item reached */ + if (_mission_type == MISSION_TYPE_OFFBOARD) { + if (!(_mission_result.seq_reached == _current_offboard_mission_index && _mission_result.reached)) { + report_mission_item_reached(); + } + } + } } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { altitude_sp_foh_update(); @@ -694,10 +705,8 @@ Mission::save_offboard_mission_state() void Mission::report_mission_item_reached() { - if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.reached = true; - _mission_result.seq_reached = _current_offboard_mission_index; - } + _mission_result.reached = true; + _mission_result.seq_reached = _current_offboard_mission_index; publish_mission_result(); } @@ -705,6 +714,8 @@ void Mission::report_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); + _mission_result.reached = false; + _mission_result.finished = false; _mission_result.seq_current = _current_offboard_mission_index; publish_mission_result(); @@ -730,7 +741,4 @@ Mission::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } - /* reset reached bool */ - _mission_result.reached = false; - _mission_result.finished = false; } From 2722921b30a48fd3fb67ba25efe8c4e3ca1338c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 12:30:29 +0200 Subject: [PATCH 5/5] Fixed function name of mission modification logic, attributed @DrTon --- src/modules/navigator/mission.cpp | 9 +++++---- src/modules/navigator/mission.h | 13 +++++++------ 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 87d16f1e67..b5926df815 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -37,6 +37,7 @@ * * @author Julian Oes * @author Thomas Gubler + * @author Anton Babushkin */ #include @@ -157,7 +158,7 @@ Mission::on_active() /* else just report that item reached */ if (_mission_type == MISSION_TYPE_OFFBOARD) { if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) { - report_mission_item_reached(); + set_mission_item_reached(); } } } @@ -701,7 +702,7 @@ Mission::save_offboard_mission_state() } void -Mission::report_mission_item_reached() +Mission::set_mission_item_reached() { _navigator->get_mission_result()->reached = true; _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; @@ -709,7 +710,7 @@ Mission::report_mission_item_reached() } void -Mission::report_current_offboard_mission_item() +Mission::set_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); _navigator->get_mission_result()->reached = false; @@ -721,7 +722,7 @@ Mission::report_current_offboard_mission_item() } void -Mission::report_mission_finished() +Mission::set_mission_finished() { _navigator->get_mission_result()->finished = true; _navigator->publish_mission_result(); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 5164737f33..ea7cc0927c 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -37,6 +37,7 @@ * * @author Julian Oes * @author Thomas Gubler + * @author Anton Babushkin */ #ifndef NAVIGATOR_MISSION_H @@ -130,19 +131,19 @@ private: void save_offboard_mission_state(); /** - * Report that a mission item has been reached + * Set a mission item as reached */ - void report_mission_item_reached(); + void set_mission_item_reached(); /** - * Rport the current mission item + * Set the current offboard mission item */ - void report_current_offboard_mission_item(); + void set_current_offboard_mission_item(); /** - * Report that the mission is finished if one exists or that none exists + * Set that the mission is finished if one exists or that none exists */ - void report_mission_finished(); + void set_mission_finished(); control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt;