diff options
Diffstat (limited to 'src/modules/navigator/mission.cpp')
-rw-r--r-- | src/modules/navigator/mission.cpp | 215 |
1 files changed, 158 insertions, 57 deletions
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c0e37a3ed..b5926df81 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -36,12 +36,15 @@ * Helper class to access missions * * @author Julian Oes <julian@oes.ch> + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <sys/types.h> #include <string.h> #include <stdlib.h> #include <unistd.h> +#include <float.h> #include <drivers/drv_hrt.h> @@ -49,6 +52,7 @@ #include <mavlink/mavlink_log.h> #include <systemlib/err.h> #include <geo/geo.h> +#include <lib/mathlib/mathlib.h> #include <uORB/uORB.h> #include <uORB/topics/mission.h> @@ -59,20 +63,23 @@ Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _param_onboard_enabled(this, "ONBOARD_EN"), - _param_takeoff_alt(this, "TAKEOFF_ALT"), - _param_dist_1wp(this, "DIST_1WP"), + _param_onboard_enabled(this, "MIS_ONBOARD_EN", false), + _param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false), + _param_dist_1wp(this, "MIS_DIST_1WP", false), + _param_altmode(this, "MIS_ALTMODE", false), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), _need_takeoff(true), _takeoff(false), - _mission_result_pub(-1), - _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), - _dist_1wp_ok(false) + _dist_1wp_ok(false), + _missionFeasiblityChecker(), + _min_current_sp_distance_xy(FLT_MAX), + _mission_item_previous_alt(NAN), + _distance_current_previous(0.0f) { /* load initial params */ updateParams(); @@ -107,6 +114,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; } @@ -141,9 +149,22 @@ 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 (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) { + set_mission_item_reached(); + } + } + } + + } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { + altitude_sp_foh_update(); } else { /* if waypoint position reached allow loiter on the setpoint */ if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { @@ -202,7 +223,7 @@ Mission::update_offboard_mission() * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt); @@ -313,11 +334,19 @@ Mission::set_mission_items() /* make sure param is up to date */ updateParams(); + /* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */ + altitude_sp_foh_reset(); + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* set previous position setpoint to current */ set_previous_pos_setpoint(); + /* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */ + if (pos_sp_triplet->previous.valid) { + _mission_item_previous_alt = _mission_item.altitude; + } + /* get home distance state */ bool home_dist_ok = check_dist_1wp(); /* the home dist check provides user feedback, so we initialize it to this */ @@ -343,6 +372,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 @@ -359,6 +392,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(); @@ -399,7 +433,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,32 +441,46 @@ 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.autocontinue = true; + _mission_item.time_inside = 0; - 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; + } + + _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 + if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) { /* try to read next mission item */ struct mission_item_s mission_item_next; @@ -444,11 +492,81 @@ Mission::set_mission_items() /* next mission item is not available */ pos_sp_triplet->next.valid = false; } + + } else { + /* vehicle will be paused on current waypoint, don't set next item */ + pos_sp_triplet->next.valid = false; + } + + /* Save the distance between the current sp and the previous one */ + if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) { + _distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, + pos_sp_triplet->current.lon, + pos_sp_triplet->previous.lat, + pos_sp_triplet->previous.lon); } _navigator->set_position_setpoint_triplet_updated(); } +void +Mission::altitude_sp_foh_update() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* Don't change setpoint if last and current waypoint are not valid */ + if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || + !isfinite(_mission_item_previous_alt)) { + return; + } + + /* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */ + if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) { + return; + } + + /* Don't do FOH for landing and takeoff waypoints, the ground may be near + * and the FW controller has a custom landing logic */ + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + return; + } + + + /* Calculate distance to current waypoint */ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + + /* Save distance to waypoint if it is the smallest ever achieved, however make sure that + * _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */ + _min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy), + _distance_current_previous); + + /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt + * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ + if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) { + pos_sp_triplet->current.alt = _mission_item.altitude; + } else { + /* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp + * of the mission item as it is used to check if the mission item is reached + * The setpoint is set linearly and such that the system reaches the current altitude at the acceptance + * radius around the current waypoint + **/ + float delta_alt = (_mission_item.altitude - _mission_item_previous_alt); + float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius); + float a = _mission_item_previous_alt - grad * _distance_current_previous; + pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; + + } + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Mission::altitude_sp_foh_reset() +{ + _min_current_sp_distance_xy = FLT_MAX; +} + bool Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item) { @@ -584,45 +702,28 @@ Mission::save_offboard_mission_state() } void -Mission::report_mission_item_reached() +Mission::set_mission_item_reached() { - if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.reached = true; - _mission_result.seq_reached = _current_offboard_mission_index; - } - publish_mission_result(); + _navigator->get_mission_result()->reached = true; + _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; + _navigator->publish_mission_result(); } void -Mission::report_current_offboard_mission_item() +Mission::set_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); - _mission_result.seq_current = _current_offboard_mission_index; - publish_mission_result(); + _navigator->get_mission_result()->reached = false; + _navigator->get_mission_result()->finished = false; + _navigator->get_mission_result()->seq_current = _current_offboard_mission_index; + _navigator->publish_mission_result(); save_offboard_mission_state(); } void -Mission::report_mission_finished() -{ - _mission_result.finished = true; - publish_mission_result(); -} - -void -Mission::publish_mission_result() +Mission::set_mission_finished() { - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); - - } else { - /* 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; + _navigator->get_mission_result()->finished = true; + _navigator->publish_mission_result(); } |