Ejemplo n.º 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();
}
Ejemplo n.º 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();
}
Ejemplo n.º 3
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();
}
Ejemplo n.º 4
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();
}