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!");
}
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");
}
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);
}
int Geofence::clearDm()
{
dm_clear(DM_KEY_FENCE_POINTS);
return OK;
}
