示例#1
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();
}
示例#2
0
void RCRecover::on_activation()
{
	_navigator->get_tracker().set_recent_path_tracking_enabled(false);
	
	/* reset starting point so we override what the triplet contained from the previous navigation state */
	_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);

	if (_state == RCRECOVER_STATE_NONE) {
		// for safety reasons don't go into RCRecover if landed
		if (_navigator->get_land_detected()->landed) {
			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RC Recover: not available when landed");

		// otherwise, return along recent path
		} else {
			_state = RCRECOVER_STATE_RETURN;
			
			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RC Recover: return along recent path");
			
			// in case there is no recent path, loiter at the current position
			loiter_lat = _navigator->get_global_position()->lat;
			loiter_lon = _navigator->get_global_position()->lon;
			loiter_alt = _navigator->get_global_position()->alt;
		}

	}

	update_mission_item();
}
示例#3
0
文件: rtl.cpp 项目: EugenSol/HippoC
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();
}
示例#4
0
void
Delivery::heading_sp_update()
{
	if (_takeoff) {
		/* we don't want to be yawing during takeoff */
		return;
	}

	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(_on_arrival_yaw)) {
		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;
	}

	/* set yaw angle for the waypoint iff a loiter time has been specified */
	if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
		_mission_item.yaw = _on_arrival_yaw;
	/* always keep the front of the rotary wing pointing to the next waypoint */
	} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
		_mission_item.yaw = get_bearing_to_next_waypoint(
		        _navigator->get_global_position()->lat,
		        _navigator->get_global_position()->lon,
		        _mission_item.lat,
		        _mission_item.lon);
	/* always keep the back of the rotary wing pointing towards home */
	} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) {
		_mission_item.yaw = get_bearing_to_next_waypoint(
		        _navigator->get_global_position()->lat,
		        _navigator->get_global_position()->lon,
		        _navigator->get_home_position()->lat,
		        _navigator->get_home_position()->lon);
	/* always keep the back of the rotary wing pointing towards home */
	} else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
		_mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint(
		        _navigator->get_global_position()->lat,
		        _navigator->get_global_position()->lon,
		        _navigator->get_home_position()->lat,
		        _navigator->get_home_position()->lon) + M_PI_F);
	}

	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
	_navigator->set_position_setpoint_triplet_updated();
}
示例#5
0
void
RCLoss::set_rcl_item()
{
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	pos_sp_triplet->previous = pos_sp_triplet->current;
	_navigator->set_can_loiter_at_sp(false);

	switch (_rcl_state) {
	case RCL_STATE_LOITER: {
			_mission_item.lat = _navigator->get_global_position()->lat;
			_mission_item.lon = _navigator->get_global_position()->lon;
			_mission_item.altitude = _navigator->get_global_position()->alt;
			_mission_item.altitude_is_relative = false;
			_mission_item.yaw = NAN;
			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get();
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			_navigator->set_can_loiter_at_sp(true);
			break;
		}

	case RCL_STATE_TERMINATE: {
			/* Request flight termination from the commander */
			_navigator->get_mission_result()->flight_termination = true;
			_navigator->set_mission_result_updated();
			warnx("rc not recovered: request flight termination");
			pos_sp_triplet->previous.valid = false;
			pos_sp_triplet->current.valid = false;
			pos_sp_triplet->next.valid = false;
			break;
		}

	default:
		break;
	}

	reset_mission_item_reached();

	/* convert mission item to current position setpoint and make it valid */
	mission_apply_limitation(_mission_item);
	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
	pos_sp_triplet->next.valid = false;

	_navigator->set_position_setpoint_triplet_updated();
}
示例#6
0
void
Takeoff::on_active()
{
    if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
        _navigator->get_mission_result()->finished = true;
        _navigator->set_mission_result_updated();

        /* set loiter item so position controllers stop doing takeoff logic */
        set_loiter_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);
        _navigator->set_position_setpoint_triplet_updated();
    }
}
示例#7
0
void
Loiter::on_activation()
{
	/* set current mission item to loiter */
	set_loiter_item(&_mission_item);

	/* convert mission item to current setpoint */
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_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);

	_navigator->set_position_setpoint_triplet_updated();
}
示例#8
0
void
EngineFailure::set_ef_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();
	_navigator->set_can_loiter_at_sp(false);

	switch (_ef_state) {
	case EF_STATE_LOITERDOWN: {
		//XXX create mission item at ground (below?) here

		_mission_item.lat = _navigator->get_global_position()->lat;
		_mission_item.lon = _navigator->get_global_position()->lon;
		_mission_item.altitude_is_relative = false;
		 //XXX setting altitude to a very low value, evaluate other options
		_mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f;
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;

		_navigator->set_can_loiter_at_sp(true);
		break;
	}
	default:
		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);
	pos_sp_triplet->next.valid = false;

	_navigator->set_position_setpoint_triplet_updated();
}
示例#9
0
void
Takeoff::on_active()
{
	struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();

	if (rep->current.valid) {
		// reset the position
		set_takeoff_position();

	} else if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
		_navigator->get_mission_result()->finished = true;
		_navigator->set_mission_result_updated();

		// set loiter item so position controllers stop doing takeoff logic
		set_loiter_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);
		_navigator->set_position_setpoint_triplet_updated();
	}
}
示例#10
0
文件: rtl.cpp 项目: DC00/Firmware
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();
}
示例#11
0
void FollowTarget::update_position_sp(bool use_velocity, bool use_position, float yaw_rate)
{
	// convert mission item to current setpoint

	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	// activate line following in pos control if position is valid

	pos_sp_triplet->previous.valid = use_position;
	pos_sp_triplet->previous = pos_sp_triplet->current;
	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
	pos_sp_triplet->current.position_valid = use_position;
	pos_sp_triplet->current.velocity_valid = use_velocity;
	pos_sp_triplet->current.vx = _current_vel(0);
	pos_sp_triplet->current.vy = _current_vel(1);
	pos_sp_triplet->next.valid = false;
	pos_sp_triplet->current.yawspeed_valid = PX4_ISFINITE(yaw_rate);
	pos_sp_triplet->current.yawspeed = yaw_rate;
	_navigator->set_position_setpoint_triplet_updated();
}
示例#12
0
void
Mission::do_abort_landing()
{
	// Abort FW landing
	//  turn the land waypoint into a loiter and stay there

	if (_mission_item.nav_cmd != NAV_CMD_LAND) {
		return;
	}

	// loiter at the larger of MIS_LTRMIN_ALT above the landing point
	//  or 2 * FW_CLMBOUT_DIFF above the current altitude
	float alt_landing = get_absolute_altitude_for_item(_mission_item);
	float alt_sp =  math::max(alt_landing + _param_loiter_min_alt.get(),
				  _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get()));

	_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
	_mission_item.altitude_is_relative = false;
	_mission_item.altitude = alt_sp;
	_mission_item.yaw = NAN;
	_mission_item.loiter_radius = _navigator->get_loiter_radius();
	_mission_item.loiter_direction = 1;
	_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
	_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
	_mission_item.autocontinue = false;
	_mission_item.origin = ORIGIN_ONBOARD;

	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

	_navigator->set_position_setpoint_triplet_updated();

	mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing)",
			 (int)(alt_sp - alt_landing));

	// move mission index back 1 (landing approach point) so that re-entering
	//  the mission doesn't try to land from the loiter above land
	// TODO: reset index to MAV_CMD_DO_LAND_START
	_current_offboard_mission_index -= 1;
}
示例#13
0
void
Takeoff::on_activation()
{
    /* set current mission item to Takeoff */
    set_takeoff_item(&_mission_item, _param_min_alt.get());
    _navigator->get_mission_result()->reached = false;
    _navigator->get_mission_result()->finished = false;
    _navigator->set_mission_result_updated();
    reset_mission_item_reached();

    /* convert mission item to current setpoint */
    struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
    pos_sp_triplet->previous.valid = false;
    mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
    pos_sp_triplet->current.yaw = _navigator->get_home_position()->yaw;
    pos_sp_triplet->current.yaw_valid = true;
    pos_sp_triplet->next.valid = false;

    _navigator->set_can_loiter_at_sp(true);

    _navigator->set_position_setpoint_triplet_updated();
}
示例#14
0
void
Loiter::set_loiter_position()
{
	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED &&
	    _navigator->get_land_detected()->landed) {

		// Not setting loiter position if disarmed and landed, instead mark the current
		// setpoint as invalid and idle (both, just to be sure).

		_navigator->set_can_loiter_at_sp(false);
		_navigator->get_position_setpoint_triplet()->current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
		_navigator->set_position_setpoint_triplet_updated();
		_loiter_pos_set = false;
		return;

	} else if (_loiter_pos_set) {
		// Already set, nothing to do.
		return;
	}

	_loiter_pos_set = true;

	// set current mission item to loiter
	set_loiter_item(&_mission_item, _param_min_alt.get());

	// convert mission item to current setpoint
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
	pos_sp_triplet->current.velocity_valid = false;
	pos_sp_triplet->previous.valid = false;
	mission_apply_limitation(_mission_item);
	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 == position_setpoint_s::SETPOINT_TYPE_LOITER);

	_navigator->set_position_setpoint_triplet_updated();
}
示例#15
0
void
Mission::heading_sp_update()
{
	if (_takeoff) {
		/* we don't want to be yawing during takeoff */
		return;
	}

	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 ||
			!PX4_ISFINITE(_on_arrival_yaw)) {
		return;
	}

	/* Don't change heading for takeoff waypoints, the ground may be near */
	if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
		return;
	}

	/* set yaw angle for the waypoint iff a loiter time has been specified */
	if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
		_mission_item.yaw = _on_arrival_yaw;
	} else {

		/* calculate direction the vehicle should point to:
		 * normal waypoint: current position to {waypoint or home or home + 180deg}
		 * landing waypoint: last waypoint to {waypoint or home or home + 180deg}
		 * For landing the last waypoint (= constant) is used to avoid excessive yawing near the ground
		 */
		double point_from_latlon[2];
		if (_mission_item.nav_cmd == NAV_CMD_LAND) {
			point_from_latlon[0] = pos_sp_triplet->previous.lat;
			point_from_latlon[1] = pos_sp_triplet->previous.lon;
		} else {
			point_from_latlon[0] = _navigator->get_global_position()->lat;
			point_from_latlon[1] = _navigator->get_global_position()->lon;
		}

		/* always keep the front of the rotary wing pointing to the next waypoint */
		if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
			_mission_item.yaw = get_bearing_to_next_waypoint(
				point_from_latlon[0],
				point_from_latlon[1],
				_mission_item.lat,
				_mission_item.lon);
		/* always keep the back of the rotary wing pointing towards home */
		} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) {
			_mission_item.yaw = get_bearing_to_next_waypoint(
				point_from_latlon[0],
				point_from_latlon[1],
				_navigator->get_home_position()->lat,
				_navigator->get_home_position()->lon);
		/* always keep the back of the rotary wing pointing towards home */
		} else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
			_mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint(
				point_from_latlon[0],
				point_from_latlon[1],
				_navigator->get_home_position()->lat,
				_navigator->get_home_position()->lon) + M_PI_F);
		}
	}

	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
	_navigator->set_position_setpoint_triplet_updated();
}
示例#16
0
void
DataLinkLoss::set_dll_item()
{
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	set_previous_pos_setpoint();
	_navigator->set_can_loiter_at_sp(false);

	switch (_dll_state) {
	case DLL_STATE_FLYTOCOMMSHOLDWP: {
		_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7;
		_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _param_commsholdalt.get();
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
		_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get();
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;

		_navigator->set_can_loiter_at_sp(true);
		break;
	}
	case DLL_STATE_FLYTOAIRFIELDHOMEWP: {
		_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7;
		_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _param_airfieldhomealt.get();
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
		_mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get();
		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;

		_navigator->set_can_loiter_at_sp(true);
		break;
	}
	case DLL_STATE_TERMINATE: {
		/* Request flight termination from the commander */
		_navigator->get_mission_result()->flight_termination = true;
		_navigator->set_mission_result_updated();
		reset_mission_item_reached();
		warnx("not switched to manual: request flight termination");
		pos_sp_triplet->previous.valid = false;
		pos_sp_triplet->current.valid = false;
		pos_sp_triplet->next.valid = false;
		break;
	}
	default:
		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);
	pos_sp_triplet->next.valid = false;

	_navigator->set_position_setpoint_triplet_updated();
}
示例#17
0
void
Takeoff::set_takeoff_position()
{
	struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();

	float abs_altitude = 0.0f;

	float min_abs_altitude;

	if (_navigator->home_position_valid()) { //only use home position if it is valid
		min_abs_altitude = _navigator->get_global_position()->alt + _param_min_alt.get();

	} else { //e.g. flow
		min_abs_altitude = _param_min_alt.get();
	}

	// Use altitude if it has been set. If home position is invalid use min_abs_altitude
	if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator->home_position_valid()) {
		abs_altitude = rep->current.alt;

		// If the altitude suggestion is lower than home + minimum clearance, raise it and complain.
		if (abs_altitude < min_abs_altitude) {
			abs_altitude = min_abs_altitude;
			mavlink_log_critical(_navigator->get_mavlink_log_pub(),
					     "Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get());
		}

	} else {
		// Use home + minimum clearance but only notify.
		abs_altitude = min_abs_altitude;
		mavlink_log_info(_navigator->get_mavlink_log_pub(),
				 "Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get());
	}


	if (abs_altitude < _navigator->get_global_position()->alt) {
		// If the suggestion is lower than our current alt, let's not go down.
		abs_altitude = _navigator->get_global_position()->alt;
		mavlink_log_critical(_navigator->get_mavlink_log_pub(),
				     "Already higher than takeoff altitude");
	}

	// set current mission item to takeoff
	set_takeoff_item(&_mission_item, abs_altitude);
	_navigator->get_mission_result()->reached = false;
	_navigator->get_mission_result()->finished = false;
	_navigator->set_mission_result_updated();
	reset_mission_item_reached();

	// convert mission item to current setpoint
	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->current.yaw_valid = true;
	pos_sp_triplet->next.valid = false;

	if (rep->current.valid) {

		// Go on and check which changes had been requested
		if (PX4_ISFINITE(rep->current.yaw)) {
			pos_sp_triplet->current.yaw = rep->current.yaw;
		}

		if (PX4_ISFINITE(rep->current.lat) && PX4_ISFINITE(rep->current.lon)) {
			pos_sp_triplet->current.lat = rep->current.lat;
			pos_sp_triplet->current.lon = rep->current.lon;
		}

		// mark this as done
		memset(rep, 0, sizeof(*rep));
	}

	_navigator->set_can_loiter_at_sp(true);

	_navigator->set_position_setpoint_triplet_updated();
}
示例#18
0
void
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 = get_absolute_altitude_for_item(_mission_item);
	}

	/* the home dist check provides user feedback, so we initialize it to this */
	bool user_feedback_done = false;

	/* try setting onboard mission item */
	if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
		/* if mission type changed, notify */
		if (_mission_type != MISSION_TYPE_ONBOARD) {
			mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
			user_feedback_done = true;
		}
		_mission_type = MISSION_TYPE_ONBOARD;

	/* try setting offboard mission item */
	} else if (read_mission_item(false, true, &_mission_item)) {
		/* if mission type changed, notify */
		if (_mission_type != MISSION_TYPE_OFFBOARD) {
			mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
			user_feedback_done = true;
		}
		_mission_type = MISSION_TYPE_OFFBOARD;
	} else {
		/* no mission available or mission finished, switch to loiter */
		if (_mission_type != MISSION_TYPE_NONE) {
			/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
			user_feedback_done = true;

			/* use last setpoint for loiter */
			_navigator->set_can_loiter_at_sp(true);

		}

		_mission_type = MISSION_TYPE_NONE;

		/* set loiter mission item and ensure that there is a minimum clearance from home */
		set_loiter_item(&_mission_item, _param_takeoff_alt.get());

		/* 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;

		/* reuse setpoint for LOITER only if it's not IDLE */
		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);

		set_mission_finished();

		if (!user_feedback_done) {
			/* only tell users that we got no mission if there has not been any
			 * better, more specific feedback yet
			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
			 */

			if (_navigator->get_vstatus()->condition_landed) {
				/* landed, refusing to take off without a mission */

				mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff");
			} else {
				mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
			}

			user_feedback_done = true;

		}

		_navigator->set_position_setpoint_triplet_updated();
		return;
	}

	if (pos_sp_triplet->current.valid) {
		_on_arrival_yaw = _mission_item.yaw;
	}

	/* do takeoff on first waypoint for rotary wing vehicles */
	if (_navigator->get_vstatus()->is_rotary_wing) {
		/* force takeoff if landed (additional protection) */
		if (!_takeoff && _navigator->get_vstatus()->condition_landed) {
			_need_takeoff = true;
		}

		/* new current mission item set, check if we need takeoff */
		if (_need_takeoff && (
				_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
				_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
				_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
				_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
				_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
				_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
			_takeoff = true;
			_need_takeoff = false;
		}
	}

	if (_takeoff) {
		/* do takeoff before going to setpoint */
		/* set mission item as next position setpoint */
		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
		/* next SP is not takeoff anymore */
		pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_POSITION;

		/* calculate takeoff altitude */
		float takeoff_alt = get_absolute_altitude_for_item(_mission_item);

		/* 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());

		} else {
			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
		}

		/* check if we already above takeoff altitude */
		if (_navigator->get_global_position()->alt < takeoff_alt) {
			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.yaw = NAN;
			_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);

			_navigator->set_position_setpoint_triplet_updated();
			return;

		} else {
			/* skip takeoff */
			_takeoff = false;
		}
	}

	if (_takeoff_finished) {
		/* we just finished takeoff */
		/* in case we still have to move to the takeoff waypoint we need a waypoint mission item */
		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
		_takeoff_finished = 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 == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
		_need_takeoff = true;
	}

	_navigator->set_can_loiter_at_sp(false);
	reset_mission_item_reached();

	if (_mission_type == MISSION_TYPE_OFFBOARD) {
		set_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;

		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 {
		/* 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();
}
示例#19
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;
}
示例#20
0
void
Delivery::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();
	_navigator->set_can_loiter_at_sp(false);

	switch (_rtl_state) {
	case RTL_STATE_CLIMB: {
		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();

		_mission_item.lat = _navigator->get_global_position()->lat;
		_mission_item.lon = _navigator->get_global_position()->lon;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = climb_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 = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;

		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d m (%d m above home)",
			(int)(climb_alt),
			(int)(climb_alt - _navigator->get_home_position()->alt));
		break;
	}

	case RTL_STATE_RETURN: {
		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;
		 // don't change altitude

		 if (pos_sp_triplet->previous.valid) {
		 	/* if previous setpoint is valid then use it to calculate heading to home */
		 	_mission_item.yaw = get_bearing_to_next_waypoint(
		 	        pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
		 	        _mission_item.lat, _mission_item.lon);

		 } else {
		 	/* else use current position */
		 	_mission_item.yaw = get_bearing_to_next_waypoint(
		 	        _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
		 	        _mission_item.lat, _mission_item.lon);
		 }
		_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 = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;

		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)",
			(int)(_mission_item.altitude),
			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
		break;
	}

	case RTL_STATE_DESCEND: {
		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
		_mission_item.time_inside = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = false;
		_mission_item.origin = ORIGIN_ONBOARD;

		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d m (%d m above home)",
			(int)(_mission_item.altitude),
			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
		break;
	}

	case RTL_STATE_LOITER: {
		bool autoland = _param_land_delay.get() > -DELAY_SIGMA;

		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = autoland;
		_mission_item.origin = ORIGIN_ONBOARD;

		_navigator->set_can_loiter_at_sp(true);

		if (autoland) {
			mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);

		} else {
			mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, loiter");
		}
		break;
	}

	case RTL_STATE_LAND: {
		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_home_position()->alt;
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_LAND;
		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
		_mission_item.time_inside = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;

		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: land at home");
		break;
	}

	case RTL_STATE_LANDED: {
		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_home_position()->alt;
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_IDLE;
		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
		_mission_item.time_inside = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;

		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, landed");
		break;
	}

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

	_navigator->set_position_setpoint_triplet_updated();
}
示例#21
0
void RCRecover::update_mission_item()
{
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	// make sure we have the latest params
	updateParams();

	if (!_start_lock)
		set_previous_pos_setpoint();

	_navigator->set_can_loiter_at_sp(false);
	
	//if (_state == RCRECOVER_STATE_LOITER)
		// todo: switch to RTL mode

	switch (_state) {
	case RCRECOVER_STATE_RETURN: {
		
		if (_navigator->get_tracker().pop_recent_path(_mission_item.lat, _mission_item.lon, _mission_item.altitude)) {
			loiter_lat = _mission_item.lat;
			loiter_lon = _mission_item.lon;
			loiter_alt = _mission_item.altitude;
			
			_mission_item.altitude_is_relative = false;
			_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 = 0.0f;
			_mission_item.pitch_min = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;
			
			_start_lock = true;
			break;
		}
		
		// Recent path is empty: fall through to loiter
		_state = RCRECOVER_STATE_LOITER;
	}

	case RCRECOVER_STATE_LOITER: {
		bool autortl = _param_rtl_delay.get() > -DELAY_SIGMA;

		_mission_item.lat = loiter_lat;
		_mission_item.lon = loiter_lon;
		_mission_item.altitude = loiter_alt;
		_mission_item.altitude_is_relative = false;
		_mission_item.yaw = _navigator->get_global_position()->yaw;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = autortl ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
		_mission_item.time_inside = _param_rtl_delay.get() < 0.0f ? 0.0f : _param_rtl_delay.get();
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = autortl;
		_mission_item.origin = ORIGIN_ONBOARD;

		_navigator->set_can_loiter_at_sp(true);

		if (autortl) {
			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RC Recover: loiter %.1fs", (double)_mission_item.time_inside);
		} else {
			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RC Recover: completed, loiter");
		}
			
		break;
	}

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

	_navigator->set_position_setpoint_triplet_updated();
}
示例#22
0
文件: rtl.cpp 项目: DC00/Firmware
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();

	if (!_rtl_start_lock) {
		set_previous_pos_setpoint();
	}

	_navigator->set_can_loiter_at_sp(false);

	switch (_rtl_state) {
	case RTL_STATE_CLIMB: {
			float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();

			_mission_item.lat = _navigator->get_global_position()->lat;
			_mission_item.lon = _navigator->get_global_position()->lon;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = climb_alt;
			_mission_item.yaw = NAN;
			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
					 (int)(climb_alt),
					 (int)(climb_alt - _navigator->get_home_position()->alt));
			break;
		}

	case RTL_STATE_RETURN: {
			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			// don't change altitude

			// use home yaw if close to home
			/* 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);

			if (home_dist < _param_rtl_min_dist.get()) {
				_mission_item.yaw = _navigator->get_home_position()->yaw;

			} else {
				if (pos_sp_triplet->previous.valid) {
					/* if previous setpoint is valid then use it to calculate heading to home */
					_mission_item.yaw = get_bearing_to_next_waypoint(
								    pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
								    _mission_item.lat, _mission_item.lon);

				} else {
					/* else use current position */
					_mission_item.yaw = get_bearing_to_next_waypoint(
								    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
								    _mission_item.lat, _mission_item.lon);
				}
			}

			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
					 (int)(_mission_item.altitude),
					 (int)(_mission_item.altitude - _navigator->get_home_position()->alt));

			_rtl_start_lock = true;
			break;
		}

	case RTL_STATE_TRANSITION_TO_MC: {
			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
			break;
		}

	case RTL_STATE_DESCEND: {
			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();

			// check if we are already lower - then we will just stay there
			if (_mission_item.altitude > _navigator->get_global_position()->alt) {
				_mission_item.altitude = _navigator->get_global_position()->alt;
			}

			_mission_item.yaw = _navigator->get_home_position()->yaw;

			// except for vtol which might be still off here and should point towards this location
			float d_current = get_distance_to_next_waypoint(
						  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
						  _mission_item.lat, _mission_item.lon);

			if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {
				_mission_item.yaw = get_bearing_to_next_waypoint(
							    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
							    _mission_item.lat, _mission_item.lon);
			}

			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = false;
			_mission_item.origin = ORIGIN_ONBOARD;

			/* disable previous setpoint to prevent drift */
			pos_sp_triplet->previous.valid = false;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
					 (int)(_mission_item.altitude),
					 (int)(_mission_item.altitude - _navigator->get_home_position()->alt));
			break;
		}

	case RTL_STATE_LOITER: {
			bool autoland = _param_land_delay.get() > -DELAY_SIGMA;

			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			// don't change altitude
			_mission_item.yaw = _navigator->get_home_position()->yaw;
			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
			_mission_item.autocontinue = autoland;
			_mission_item.origin = ORIGIN_ONBOARD;

			_navigator->set_can_loiter_at_sp(true);

			if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
						 (double)Navigator::get_time_inside(_mission_item));

			} else {
				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
			}

			break;
		}

	case RTL_STATE_LAND: {
			_mission_item.yaw = _navigator->get_home_position()->yaw;
			set_land_item(&_mission_item, false);

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
			break;
		}

	case RTL_STATE_LANDED: {
			set_idle_item(&_mission_item);

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
			break;
		}

	default:
		break;
	}

	reset_mission_item_reached();

	/* execute command if set */
	if (!item_contains_position(&_mission_item)) {
		issue_command(&_mission_item);
	}

	/* 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();
}
示例#23
0
void
Mission::set_mission_items()
{
	/* make sure param is up to date */
	updateParams();

	/* reset the altitude foh (first order hold) 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();

	/* the home dist check provides user feedback, so we initialize it to this */
	bool user_feedback_done = false;

	/* mission item that comes after current if available */
	struct mission_item_s mission_item_next_position;
	bool has_next_position_item = false;

	work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;

	/* copy information about the previous mission item */
	if (item_contains_position(&_mission_item) && pos_sp_triplet->current.valid) {
		/* Copy previous mission item altitude */
		_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
	}

	/* try setting onboard mission item */
	if (_param_onboard_enabled.get()
	    && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {

		/* if mission type changed, notify */
		if (_mission_type != MISSION_TYPE_ONBOARD) {
			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing internal mission");
			user_feedback_done = true;
		}

		_mission_type = MISSION_TYPE_ONBOARD;

		/* try setting offboard mission item */

	} else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
		/* if mission type changed, notify */
		if (_mission_type != MISSION_TYPE_OFFBOARD) {
			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing mission");
			user_feedback_done = true;
		}

		_mission_type = MISSION_TYPE_OFFBOARD;

	} else {
		/* no mission available or mission finished, switch to loiter */
		if (_mission_type != MISSION_TYPE_NONE) {

			if (_navigator->get_land_detected()->landed) {
				mavlink_log_info(_navigator->get_mavlink_log_pub(), "mission finished, landed");

			} else {
				/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
				mavlink_log_info(_navigator->get_mavlink_log_pub(), "mission finished, loitering");

				/* use last setpoint for loiter */
				_navigator->set_can_loiter_at_sp(true);
			}

			user_feedback_done = true;
		}

		_mission_type = MISSION_TYPE_NONE;

		/* set loiter mission item and ensure that there is a minimum clearance from home */
		set_loiter_item(&_mission_item, _param_takeoff_alt.get());

		/* 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;

		/* reuse setpoint for LOITER only if it's not IDLE */
		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);

		set_mission_finished();

		if (!user_feedback_done) {
			/* only tell users that we got no mission if there has not been any
			 * better, more specific feedback yet
			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
			 */

			if (_navigator->get_land_detected()->landed) {
				/* landed, refusing to take off without a mission */

				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff");

			} else {
				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, loitering");
			}

			user_feedback_done = true;

		}

		_navigator->set_position_setpoint_triplet_updated();
		return;
	}

	/*********************************** handle mission item *********************************************/

	/* handle position mission items */


	if (item_contains_position(&_mission_item)) {

		/* force vtol land */
		if (_mission_item.nav_cmd == NAV_CMD_LAND && _navigator->get_vstatus()->is_vtol
		    && _param_force_vtol.get() && !_navigator->get_vstatus()->is_rotary_wing) {

			_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
		}

		/* we have a new position item so set previous position setpoint to current */
		set_previous_pos_setpoint();

		/* do takeoff before going to setpoint if needed and not already in takeoff */
		if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {

			new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;

			/* use current mission item as next position item */
			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
			mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT;
			has_next_position_item = true;

			float takeoff_alt = calculate_takeoff_altitude(&_mission_item);

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "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;
			/* ignore yaw for takeoff items */
			_mission_item.yaw = NAN;
			_mission_item.altitude = takeoff_alt;
			_mission_item.altitude_is_relative = false;
			_mission_item.autocontinue = true;
			_mission_item.time_inside = 0;
		}

		/* if we just did a takeoff navigate to the actual waypoint now */
		if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {

			if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
			    && _navigator->get_vstatus()->is_rotary_wing
			    && !_navigator->get_land_detected()->landed
			    && has_next_position_item) {

				/* check if the vtol_takeoff command is on top of us */
				if (do_need_move_to_takeoff()) {
					new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;

				} else {
					new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
				}


				_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
				_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
				_mission_item.yaw = _navigator->get_global_position()->yaw;

			} else {
				new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
				/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
				_mission_item.yaw = NAN;
			}

		}

		/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
		if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
		    && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {

			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.autocontinue = true;
			_mission_item.time_inside = 0.0f;
		}

		/* move to land wp as fixed wing */
		if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
		    && _work_item_type == WORK_ITEM_TYPE_DEFAULT
		    && !_navigator->get_land_detected()->landed) {

			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
			/* use current mission item as next position item */
			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
			has_next_position_item = true;
			float altitude = _navigator->get_global_position()->alt;

			if (pos_sp_triplet->current.valid) {
				altitude = pos_sp_triplet->current.alt;
			}

			_mission_item.altitude = altitude;
			_mission_item.altitude_is_relative = false;
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.autocontinue = true;
			_mission_item.time_inside = 0;
		}

		/* transition to MC */
		if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
		    && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
		    && !_navigator->get_vstatus()->is_rotary_wing
		    && !_navigator->get_land_detected()->landed) {

			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
			_mission_item.autocontinue = true;
			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
		}

		/* move to landing waypoint before descent if necessary */
		if (do_need_move_to_land() &&
		    (_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
		     _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {

			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;

			/* use current mission item as next position item */
			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
			has_next_position_item = true;

			/*
			 * Ignoring waypoint altitude:
			 * Set altitude to the same as we have now to prevent descending too fast into
			 * the ground. Actual landing will descend anyway until it touches down.
			 * XXX: We might want to change that at some point if it is clear to the user
			 * what the altitude means on this waypoint type.
			 */
			float altitude = _navigator->get_global_position()->alt;

			_mission_item.altitude = altitude;
			_mission_item.altitude_is_relative = false;
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.autocontinue = true;
			_mission_item.time_inside = 0;
		}

		/* we just moved to the landing waypoint, now descend */
		if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
		    && _navigator->get_vstatus()->is_rotary_wing) {

			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
		}



		/* ignore yaw for landing items */
		/* XXX: if specified heading for landing is desired we could add another step before the descent
		 * that aligns the vehicle first */
		if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
			_mission_item.yaw = NAN;
		}

		/* handle non-position mission items such as commands */

	} else {

		/* turn towards next waypoint before MC to FW transition */
		if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
		    && _work_item_type != WORK_ITEM_TYPE_ALIGN
		    && _navigator->get_vstatus()->is_rotary_wing
		    && !_navigator->get_land_detected()->landed
		    && has_next_position_item) {

			new_work_item_type = WORK_ITEM_TYPE_ALIGN;
			set_align_mission_item(&_mission_item, &mission_item_next_position);
		}

		/* yaw is aligned now */
		if (_work_item_type == WORK_ITEM_TYPE_ALIGN) {
			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
			set_previous_pos_setpoint();
		}

	}

	/*********************************** set setpoints and check next *********************************************/

	/* set current position setpoint from mission item (is protected against non-position items) */
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

	/* issue command if ready (will do nothing for position mission items) */
	issue_command(&_mission_item);

	/* set current work item type */
	_work_item_type = new_work_item_type;

	/* require takeoff after landing or idle */
	if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
	    || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {

		_need_takeoff = true;
	}

	_navigator->set_can_loiter_at_sp(false);
	reset_mission_item_reached();

	if (_mission_type == MISSION_TYPE_OFFBOARD) {
		set_current_offboard_mission_item();
	}

	// TODO: report onboard mission item somehow

	if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
		/* try to process next mission item */

		if (has_next_position_item) {
			/* got next mission item, update setpoint triplet */
			mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);

		} else {
			/* 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();
}