From ee254be845f16531990f4574ca7505ed359c4a61 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 11:16:27 +0200 Subject: navigator: skip takeoff if already above takeoff altitude --- src/modules/navigator/mission.cpp | 67 +++++++++++++++++++++++---------------- 1 file changed, 39 insertions(+), 28 deletions(-) (limited to 'src/modules/navigator/mission.cpp') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c0e37a3ed..98a4340e1 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,43 +408,53 @@ 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); + _navigator->set_position_setpoint_triplet_updated(); + return; - /* 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; + } else { + /* skip takeoff */ + _takeoff = false; } + } - _navigator->set_can_loiter_at_sp(false); - reset_mission_item_reached(); + /* set current position setpoint from mission item */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - if (_mission_type == MISSION_TYPE_OFFBOARD) { - report_current_offboard_mission_item(); - } - // TODO: report onboard mission item somehow + /* 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; + } - /* try to read next mission item */ - struct mission_item_s mission_item_next; + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); - 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 (_mission_type == MISSION_TYPE_OFFBOARD) { + report_current_offboard_mission_item(); + } + // TODO: report onboard mission item somehow - } else { - /* next mission item is not available */ - pos_sp_triplet->next.valid = false; - } + /* 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(); -- cgit v1.2.3 From 7b4448510436ddb3d5c548a4039bfa53c3aabfc4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 18:46:42 +0200 Subject: mission: takeoff and next waypoint related fixes --- src/modules/navigator/mission.cpp | 35 ++++++++++++++++++++++++----------- 1 file changed, 24 insertions(+), 11 deletions(-) (limited to 'src/modules/navigator/mission.cpp') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 98a4340e1..128dab7d7 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; } -- cgit v1.2.3 From cf4604f5c3614bc5cd1b82dfee693cbac36181ab Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Sep 2014 13:34:44 +0400 Subject: navigator/mission.cpp: indentation fixed --- src/modules/navigator/mission.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) (limited to 'src/modules/navigator/mission.cpp') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 128dab7d7..719301a0a 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); -- cgit v1.2.3 From 70e5d4027a3b1465d5128dbf9a04cbb6545e043d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 30 Sep 2014 09:08:31 +0400 Subject: navigator: autocontinue fix --- src/modules/navigator/mission.cpp | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) (limited to 'src/modules/navigator/mission.cpp') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 104065132..8bacaa425 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; } -- cgit v1.2.3 From 2722921b30a48fd3fb67ba25efe8c4e3ca1338c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 12:30:29 +0200 Subject: 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(-) (limited to 'src/modules/navigator/mission.cpp') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 87d16f1e6..b5926df81 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 5164737f3..ea7cc0927 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; -- cgit v1.2.3 From 2587074a07f67ed37c8d4f0b4d7f1b31bb2f6525 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 13:07:56 +0200 Subject: Compile fixes in navigator --- src/modules/navigator/mission.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/navigator/mission.cpp') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index b5926df81..7fac69a61 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -234,7 +234,7 @@ Mission::update_offboard_mission() _current_offboard_mission_index = 0; } - report_current_offboard_mission_item(); + set_current_offboard_mission_item(); } @@ -396,7 +396,7 @@ Mission::set_mission_items() _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); reset_mission_item_reached(); - report_mission_finished(); + set_mission_finished(); _navigator->set_position_setpoint_triplet_updated(); return; @@ -476,7 +476,7 @@ Mission::set_mission_items() reset_mission_item_reached(); if (_mission_type == MISSION_TYPE_OFFBOARD) { - report_current_offboard_mission_item(); + set_current_offboard_mission_item(); } // TODO: report onboard mission item somehow -- cgit v1.2.3