Example #1
0
void Search::ClearAll() {
	warnx("Clearing Onboard Waypoints!");
	dm_clear(DM_KEY_WAYPOINTS_ONBOARD);
	dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0);
	offboard = (offboard == 0) ? 1 : 0;
	/*dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0);
	dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1);*/
	warnx("Finished clearing all Waypoints!");
}
Example #2
0
void
Geofence::addPoint(int argc, char *argv[])
{
	int ix, last;
	double lon, lat;
	struct fence_vertex_s vertex;
	char *end;

	if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) {
		dm_clear(DM_KEY_FENCE_POINTS);
		publishFence(0);
		return;
	}

	if (argc < 3) {
		PX4_WARN("Specify: -clear | sequence latitude longitude [-publish]");
	}

	ix = atoi(argv[0]);

	if (ix >= DM_KEY_FENCE_POINTS_MAX) {
		PX4_WARN("Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX);
	}

	lat = strtod(argv[1], &end);
	lon = strtod(argv[2], &end);

	last = 0;

	if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) {
		last = 1;
	}

	vertex.lat = (float)lat;
	vertex.lon = (float)lon;

	if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) {
		if (last) {
			publishFence((unsigned)ix + 1);
		}

		return;
	}

	PX4_WARN("can't store fence point");
}
Example #3
0
void Search::SetMission(double search_radius) {
	// Get the amount of waypoints needed for mission.
	// Note: mission should already have a size of 1. For endpoint.
	// Note: we won't need to manage the home waypoint ourselves.
	// Note: We assume  the last waypoint is the upper left corner of the square search pattern.
	// Note: Only test on the field where the home position is correctly set
	// Home position is not part of mission, Index 0 = First new waypoint

	/* Waypoint ID 
	 * Waypoint Count = Waypoint ID + 1*/
	dm_clear(DM_KEY_WAYPOINTS_OFFBOARD(offboard));
	offboard = 0;
	int wp_id = 0;
	// Note: Acceptance Radius not accounted for (it's in meters, not degrees))
	while (next_point.lat - last.lat > search_radius && next_point.lon - last.lon > search_radius) {
		if (wp_id % 2 == 0) {
			// Do Latitudinal shifts here
			if (next_point.lat - last.lat < search_radius) {
				// Going Up
				next_point.lat = last.lat;
			} else {
				// Going down
				next_point.lat = home.lat;
			}
		} else {
			// Do longitudinal shifts here
			next_point.lon += -1 * search_radius;
		}
		if (wp_id)
			next_point.nav_cmd = NAV_CMD_WAYPOINT;
		dm_write(DM_KEY_WAYPOINTS_OFFBOARD(offboard), wp_id++, DM_PERSIST_POWER_ON_RESET, &next_point, sizeof(struct mission_item_s));
		offboard = (offboard == 0) ? 1 : 0;
	}
	search_mission.count = wp_id + 1;
	warnx("Waypoint count is: %d", search_mission.count);
	search_mission.current_seq = 0;
	orb_publish(ORB_ID(offboard_mission), mission_pub, &search_mission);
}
Example #4
0
int Geofence::clearDm()
{
	dm_clear(DM_KEY_FENCE_POINTS);
	return OK;
}