Exemple #1
0
void
Delivery::return_home()
{
	// turn on RTL with landing enabled
	// should halt/switch to loiter if collision_imminent is true
	// status = return ; change to complete

	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
		advance_rtl();
		set_rtl_item();
	}

	if (_rtl_state == RTL_STATE_LAND) {
		_count++;
		if (_count >= 3000) {
			advance_rtl();
		}
	}

	if (_rtl_state == RTL_STATE_LANDED) {
		// Update Status now that return is complete
		advance_delivery();
		mavlink_log_critical(_navigator->get_mavlink_fd(), "Black Hawk Has Nested");
	}
}
Exemple #2
0
void
RTL::on_activation()
{
	/* reset starting point so we override what the triplet contained from the previous navigation state */
	_rtl_start_lock = false;
	set_current_position_item(&_mission_item);
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

	/* decide where to enter the RTL procedure when we switch into it */
	if (_rtl_state == RTL_STATE_NONE) {
		/* for safety reasons don't go into RTL if landed */
		if (_navigator->get_vstatus()->condition_landed) {
			_rtl_state = RTL_STATE_LANDED;
			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no RTL when landed");

		/* if lower than return altitude, climb up first */
		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
			   + _param_return_alt.get()) {
			_rtl_state = RTL_STATE_CLIMB;

		/* otherwise go straight to return */
		} else {
			/* set altitude setpoint to current altitude */
			_rtl_state = RTL_STATE_RETURN;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = _navigator->get_global_position()->alt;
		}

	}

	set_rtl_item();
}
Exemple #3
0
void
RTL::on_activation()
{
    /* decide where to enter the RTL procedure when we switch into it */
    if (_rtl_state == RTL_STATE_NONE) {
        /* for safety reasons don't go into RTL if landed */
        if (_navigator->get_vstatus()->condition_landed) {
            _rtl_state = RTL_STATE_LANDED;
            mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");

            /* if lower than return altitude, climb up first */
        } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
                   + _param_return_alt.get()) {
            _rtl_state = RTL_STATE_CLIMB;

            /* otherwise go straight to return */
        } else {
            /* set altitude setpoint to current altitude */
            _rtl_state = RTL_STATE_RETURN;
            _mission_item.altitude_is_relative = false;
            _mission_item.altitude = _navigator->get_global_position()->alt;
        }
    }

    set_rtl_item();
}
Exemple #4
0
void
RTL::on_activation()
{
	set_current_position_item(&_mission_item);
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
	pos_sp_triplet->previous.valid = false;
	pos_sp_triplet->next.valid = false;

	/* for safety reasons don't go into RTL if landed */
	if (_navigator->get_land_detected()->landed) {
		_rtl_state = RTL_STATE_LANDED;
		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");

		/* if lower than return altitude, climb up first */

	} else if (_navigator->get_global_position()->alt < (_navigator->get_home_position()->alt
			+ get_rtl_altitude())) {
		_rtl_state = RTL_STATE_CLIMB;

		/* otherwise go straight to return */

	} else {
		/* set altitude setpoint to current altitude */
		_rtl_state = RTL_STATE_RETURN;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_global_position()->alt;
	}

	set_rtl_item();
}
Exemple #5
0
void
RTL::on_active()
{
    if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
        advance_rtl();
        set_rtl_item();
    }
}
Exemple #6
0
void
Navigator::on_mission_item_reached()
{
	if (myState == NAV_STATE_MISSION) {

		_mission.report_mission_item_reached();

		if (_do_takeoff) {
			/* takeoff completed */
			_do_takeoff = false;
			mavlink_log_info(_mavlink_fd, "#audio: takeoff completed");

		} else {
			/* advance by one mission item */
			_mission.move_to_next();
		}

		if (_mission.current_mission_available()) {
			set_mission_item();

		} else {
			/* if no more mission items available then finish mission */
			/* loiter at last waypoint */
			_reset_loiter_pos = false;
			mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
			request_loiter_or_ready();
		}

	} else if (myState == NAV_STATE_RTL) {
		/* RTL completed */
		if (_rtl_state == RTL_STATE_DESCEND) {
			/* hovering above home position, land if needed or loiter */
			mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");

			if (_mission_item.autocontinue) {
				dispatch(EVENT_LAND_REQUESTED);

			} else {
				_reset_loiter_pos = false;
				dispatch(EVENT_LOITER_REQUESTED);
			}

		} else {
			/* next RTL step */
			_rtl_state = (RTLState)(_rtl_state + 1);
			set_rtl_item();
		}

	} else if (myState == NAV_STATE_LAND) {
		/* landing completed */
		mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
		dispatch(EVENT_READY_REQUESTED);
	}
	_mission.publish_mission_result();
}
Exemple #7
0
void
RTL::on_activation()
{
	/* reset starting point so we override what the triplet contained from the previous navigation state */
	_rtl_start_lock = false;
	set_current_position_item(&_mission_item);
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

	/* check if we are pretty close to home already */
	float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
			  _navigator->get_home_position()->lon,
			  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);

	/* decide where to enter the RTL procedure when we switch into it */
	if (_rtl_state == RTL_STATE_NONE) {
		/* for safety reasons don't go into RTL if landed */
		if (_navigator->get_land_detected()->landed) {
			_rtl_state = RTL_STATE_LANDED;
			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");

			/* if lower than return altitude, climb up first */

		} else if (home_dist > _param_rtl_min_dist.get()
			   && _navigator->get_global_position()->alt < _navigator->get_home_position()->alt
			   + _param_return_alt.get()) {
			_rtl_state = RTL_STATE_CLIMB;

			/* otherwise go straight to return */

		} else {
			/* set altitude setpoint to current altitude */
			_rtl_state = RTL_STATE_RETURN;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = _navigator->get_global_position()->alt;
		}

	}

	set_rtl_item();
}
Exemple #8
0
void
Navigator::start_rtl()
{
	_do_takeoff = false;

	/* decide if we need climb */
	if (_rtl_state == RTL_STATE_NONE) {
		if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) {
			_rtl_state = RTL_STATE_CLIMB;

		} else {
			_rtl_state = RTL_STATE_RETURN;
		}
	}

	/* if switching directly to return state, reset altitude setpoint */
	if (_rtl_state == RTL_STATE_RETURN) {
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _global_pos.alt;
	}

	_reset_loiter_pos = true;
	set_rtl_item();
}
Exemple #9
0
void
Delivery::set_delivery_items()
{
	// set the parameters needed for getting to the destination
	if (delivery_status == DELIV_ENROUTE) {

		set_mission_items();

	} else if (delivery_status == DELIV_DROPOFF) {

		mavlink_log_critical(_navigator->get_mavlink_fd(), "Setting descent mission item");

		// setting the mission items for a descent to the _drop_alt
		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();
		_navigator->set_can_loiter_at_sp(false);

		// mission_item to descend to _drop_alt
		_mission_item.lat = _navigator->get_global_position()->lat;
		_mission_item.lon = _navigator->get_global_position()->lon;
		_mission_item.altitude_is_relative = false;
		// .altitude needs to be referenced to location altitude, not home altitude
		_mission_item.altitude = _navigator->get_home_position()->alt + _drop_alt;
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
		_mission_item.time_inside = 10.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = false;
		_mission_item.origin = ORIGIN_ONBOARD;

		mavlink_log_critical(_navigator->get_mavlink_fd(), "DELIVERY: descend to %d m (%d m above destination)",
		(int)(_mission_item.altitude),
		(int)(_mission_item.altitude - _navigator->get_home_position()->alt));

		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);
		pos_sp_triplet->next.valid = false;

		_navigator->set_position_setpoint_triplet_updated();

	} else if (delivery_status == DELIV_RETURN) {

		// setting the proper _rtl_state for current position
		if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
				   + _param_return_alt.get()) {
			_rtl_state = RTL_STATE_CLIMB;

			/* otherwise go straight to return */
		} else {
			/* set altitude setpoint to current altitude */
			_rtl_state = RTL_STATE_RETURN;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = _navigator->get_global_position()->alt;
		}

		set_rtl_item();

	}

	_first_run = false;
	_complete = false;
}