bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing,
	dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
	float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued,
	float default_acceptance_rad,
	bool condition_landed)
{
	bool failed = false;
	bool warned = false;
	/* Init if not done yet */
	init();

	_mavlink_fd = mavlink_fd;

	// first check if we have a valid position
	if (!home_valid /* can later use global / local pos for finer granularity */) {
		failed = true;
		warned = true;
		mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock.");
	} else {
		failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
	}

	// check if all mission item commands are supported
	failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, condition_landed);
	failed = failed || !checkGeofence(dm_current, nMissionItems, geofence);
	failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);

	if (isRotarywing) {
		failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad);
	} else {
		failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
	}

	return !failed;
}
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{

	/* Perform checks and issue feedback to the user for all checks */
	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);

	/* Mission is only marked as feasible if all checks return true */
	return (resGeofence && resHomeAltitude);
}
bool
MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
		float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
		bool land_start_req)
{
	dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
	const size_t nMissionItems = mission.count;

	const bool isRotarywing = (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol);

	Geofence &geofence = _navigator->get_geofence();
	fw_pos_ctrl_status_s *fw_pos_ctrl_status = _navigator->get_fw_pos_ctrl_status();
	const float home_alt = _navigator->get_home_position()->alt;
	const bool home_valid = _navigator->home_position_valid();
	const bool home_alt_valid = _navigator->home_alt_valid();

	bool &warning_issued = _navigator->get_mission_result()->warning;
	const float default_acceptance_rad  = _navigator->get_default_acceptance_radius();
	const float default_altitude_acceptance_rad  = _navigator->get_altitude_acceptance_radius();
	const bool landed = _navigator->get_land_detected()->landed;

	bool failed = false;
	bool warned = false;

	// first check if we have a valid position
	if (!home_alt_valid) {
		failed = true;
		warned = true;
		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.");

	} else {
		failed = failed ||
			 !checkDistanceToFirstWaypoint(dm_current, nMissionItems, max_distance_to_1st_waypoint, warning_issued);
	}

	// check if all mission item commands are supported
	failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, landed);
	failed = failed
		 || !checkDistancesBetweenWaypoints(dm_current, nMissionItems, max_distance_between_waypoints, warning_issued);
	failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt, home_valid);
	failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_alt_valid, warned);

	if (isRotarywing) {
		failed = failed
			 || !checkRotarywing(dm_current, nMissionItems, home_alt, home_alt_valid, default_altitude_acceptance_rad);

	} else {
		failed = failed
			 || !checkFixedwing(dm_current, nMissionItems, fw_pos_ctrl_status, home_alt, home_alt_valid,
					    default_acceptance_rad, land_start_req);
	}

	return !failed;
}
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
	/* Update fixed wing navigation capabilites */
	updateNavigationCapabilities();
//	warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);

	/* Perform checks and issue feedback to the user for all checks */
	bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);

	/* Mission is only marked as feasible if all checks return true */
	return (resLanding && resGeofence && resHomeAltitude);
}
bool
MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
		float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
		bool land_start_req)
{
	bool failed = false;
	bool warned = false;

	// first check if we have a valid position
	const bool home_valid = _navigator->home_position_valid();
	const bool home_alt_valid = _navigator->home_alt_valid();

	if (!home_alt_valid) {
		failed = true;
		warned = true;
		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.");

	} else {
		failed = failed || !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint);
	}

	const float home_alt = _navigator->get_home_position()->alt;

	// check if all mission item commands are supported
	failed = failed || !checkMissionItemValidity(mission);
	failed = failed || !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints);
	failed = failed || !checkGeofence(mission, home_alt, home_valid);
	failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid, warned);

	// VTOL always respects rotary wing feasibility
	if (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol) {
		failed = failed || !checkRotarywing(mission, home_alt, home_alt_valid);

	} else {
		failed = failed || !checkFixedwing(mission, home_alt, home_alt_valid, land_start_req);
	}

	return !failed;
}
Ejemplo n.º 6
0
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
	/* Update fixed wing navigation capabilites */
	updateNavigationCapabilities();
//	warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);

	return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
}
Ejemplo n.º 7
0
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{

	return  (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
}