bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
	/* Check if all mission items are inside the geofence (if we have a valid geofence) */
	if (geofence.valid()) {
		for (size_t i = 0; i < nMissionItems; i++) {
			struct mission_item_s missionitem;
			const ssize_t len = sizeof(missionitem);

			if (dm_read(dm_current, i, &missionitem, len) != len) {
				/* not supposed to happen unless the datamanager can't access the SD card, etc. */
				return false;
			}

			// Geofence function checks against home altitude amsl
			missionitem.altitude = missionitem.altitude_is_relative
				      ? missionitem.altitude + home_alt
			              : missionitem.altitude;

			if (MissionBlock::item_contains_position(&missionitem) &&
				!geofence.inside(missionitem)) {

				mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i + 1);
				return false;
			}
		}
	}

	return true;
}
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
{
	/* Check if all mission items are inside the geofence (if we have a valid geofence) */
	if (geofence.valid()) {
		for (size_t i = 0; i < nMissionItems; i++) {
			struct mission_item_s missionitem;
			const ssize_t len = sizeof(missionitem);

			if (dm_read(dm_current, i, &missionitem, len) != len) {
				/* not supposed to happen unless the datamanager can't access the SD card, etc. */
				return false;
			}

			if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
				mavlink_log_critical(_mavlink_fd, "Geofence violation for waypoint %d", i);
				return false;
			}
		}
	}

	return true;
}
Ejemplo n.º 3
0
void
Navigator::parameters_update()
{
	/* read from param to clear updated flag */
	struct parameter_update_s update;
	orb_copy(ORB_ID(parameter_update), _params_sub, &update);

	param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude));
	param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius));
	param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
	param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled));
	param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));
	param_get(_parameter_handles.land_alt, &(_parameters.land_alt));
	param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt));
	param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay));

	_mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled);

	_geofence.updateParams();
}
Ejemplo n.º 4
0
void
Navigator::task_main()
{
	/* inform about start */
	warnx("Initializing..");
	fflush(stdout);

	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);

	mavlink_log_info(_mavlink_fd, "[navigator] started");

	/* Try to load the geofence:
	 * if /fs/microsd/etc/geofence.txt load from this file
	 * else clear geofence data in datamanager
	 */
	struct stat buffer;

	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
		warnx("Try to load geofence.txt");
		_geofence.loadFromFile(GEOFENCE_FILENAME);

	} else {
		if (_geofence.clearDm() > 0)
			warnx("Geofence cleared");
		else
			warnx("Could not clear geofence");
	}

	/*
	 * do subscriptions
	 */
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	_offboard_mission_sub = orb_subscribe(ORB_ID(mission));
	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_home_pos_sub = orb_subscribe(ORB_ID(home_position));

	/* copy all topics first time */
	vehicle_status_update();
	vehicle_control_mode_update();
	parameters_update();
	global_position_update();
	home_position_update();
	navigation_capabilities_update();
	offboard_mission_update(_vstatus.is_rotary_wing);
	onboard_mission_update();

	/* rate limit position updates to 50 Hz */
	orb_set_interval(_global_pos_sub, 20);

	unsigned prevState = NAV_STATE_NONE;
	hrt_abstime mavlink_open_time = 0;
	const hrt_abstime mavlink_open_interval = 500000;

	/* wakeup source(s) */
	struct pollfd fds[8];

	/* Setup of loop */
	fds[0].fd = _params_sub;
	fds[0].events = POLLIN;
	fds[1].fd = _global_pos_sub;
	fds[1].events = POLLIN;
	fds[2].fd = _home_pos_sub;
	fds[2].events = POLLIN;
	fds[3].fd = _capabilities_sub;
	fds[3].events = POLLIN;
	fds[4].fd = _offboard_mission_sub;
	fds[4].events = POLLIN;
	fds[5].fd = _onboard_mission_sub;
	fds[5].events = POLLIN;
	fds[6].fd = _vstatus_sub;
	fds[6].events = POLLIN;
	fds[7].fd = _control_mode_sub;
	fds[7].events = POLLIN;

	while (!_task_should_exit) {

		/* wait for up to 100ms for data */
		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		/* timed out - periodic check for _task_should_exit, etc. */
		if (pret == 0) {
			continue;
		}

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			continue;
		}

		perf_begin(_loop_perf);

		if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
			/* try to reopen the mavlink log device with specified interval */
			mavlink_open_time = hrt_abstime() + mavlink_open_interval;
			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
		}

		/* vehicle control mode updated */
		if (fds[7].revents & POLLIN) {
			vehicle_control_mode_update();
		}

		/* vehicle status updated */
		if (fds[6].revents & POLLIN) {
			vehicle_status_update();

			/* evaluate state machine from commander and set the navigator mode accordingly */
			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
				bool stick_mode = false;

				if (!_vstatus.rc_signal_lost) {
					/* RC signal available, use control switches to set mode */
					/* RETURN switch, overrides MISSION switch */
					if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
						/* switch to RTL if not already landed after RTL and home position set */
						if (!(_rtl_state == RTL_STATE_DESCEND &&
						      (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
						    _vstatus.condition_home_position_valid) {
							dispatch(EVENT_RTL_REQUESTED);
						}

						stick_mode = true;

					} else {
						/* MISSION switch */
						if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
							request_loiter_or_ready();
							stick_mode = true;

						} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
							request_mission_if_available();
							stick_mode = true;
						}

						if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
							/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
							request_mission_if_available();
							stick_mode = true;
						}
					}
				}

				if (!stick_mode) {
					if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
						/* commander requested new navigation mode, try to set it */
						_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;

						switch (_vstatus.set_nav_state) {
						case NAV_STATE_NONE:
							/* nothing to do */
							break;

						case NAV_STATE_LOITER:
							request_loiter_or_ready();
							break;

						case NAV_STATE_MISSION:
							request_mission_if_available();
							break;

						case NAV_STATE_RTL:
							if (!(_rtl_state == RTL_STATE_DESCEND &&
							      (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
							    _vstatus.condition_home_position_valid) {
								dispatch(EVENT_RTL_REQUESTED);
							}

							break;

						case NAV_STATE_LAND:
							dispatch(EVENT_LAND_REQUESTED);

							break;

						default:
							warnx("ERROR: Requested navigation state not supported");
							break;
						}

					} else {
						/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
						if (myState == NAV_STATE_NONE) {
							request_mission_if_available();
						}
					}
				}

			} else {
				/* navigator shouldn't act */
				dispatch(EVENT_NONE_REQUESTED);
			}
		}

		/* parameters updated */
		if (fds[0].revents & POLLIN) {
			parameters_update();
			/* note that these new parameters won't be in effect until a mission triplet is published again */
		}

		/* navigation capabilities updated */
		if (fds[3].revents & POLLIN) {
			navigation_capabilities_update();
		}

		/* offboard mission updated */
		if (fds[4].revents & POLLIN) {
			offboard_mission_update(_vstatus.is_rotary_wing);
			// XXX check if mission really changed
			dispatch(EVENT_MISSION_CHANGED);
		}

		/* onboard mission updated */
		if (fds[5].revents & POLLIN) {
			onboard_mission_update();
			// XXX check if mission really changed
			dispatch(EVENT_MISSION_CHANGED);
		}

		/* home position updated */
		if (fds[2].revents & POLLIN) {
			home_position_update();
			// XXX check if home position really changed
			dispatch(EVENT_HOME_POSITION_CHANGED);
		}

		/* global position updated */
		if (fds[1].revents & POLLIN) {
			global_position_update();

			/* publish position setpoint triplet on each position update if navigator active */
			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
				_pos_sp_triplet_updated = true;

				if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) {
					/* got global position when landing, update setpoint */
					start_land();
				}

				_global_pos_valid = _global_pos.global_valid;

				/* check if waypoint has been reached in MISSION, RTL and LAND modes */
				if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
					if (check_mission_item_reached()) {
						on_mission_item_reached();
					}
				}
			}

			/* Check geofence violation */
			if (!_geofence.inside(&_global_pos)) {
				//xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination)

				/* Issue a warning about the geofence violation once */
				if (!_geofence_violation_warning_sent) {
					mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
					_geofence_violation_warning_sent = true;
				}

			} else {
				/* Reset the _geofence_violation_warning_sent field */
				_geofence_violation_warning_sent = false;
			}
		}

		/* publish position setpoint triplet if updated */
		if (_pos_sp_triplet_updated) {
			_pos_sp_triplet_updated = false;
			publish_position_setpoint_triplet();
		}

		/* notify user about state changes */
		if (myState != prevState) {
			mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]);
			prevState = myState;
		}

		perf_end(_loop_perf);
	}

	warnx("exiting.");

	_navigator_task = -1;
	_exit(0);
}
Ejemplo n.º 5
0
void Navigator::load_fence_from_file(const char *filename)
{
	_geofence.loadFromFile(filename);
}
Ejemplo n.º 6
0
void Navigator::add_fence_point(int argc, char *argv[])
{
	_geofence.addPoint(argc, argv);
}