/* * polygon tests */ void setup(void) { unsigned i, count; bool all_passed = true; uint32_t start_time; hal.console->println("polygon unit tests\n"); if (!Polygon_complete(OBC_boundary, ARRAY_SIZE(OBC_boundary))) { hal.console->println("OBC boundary is not complete!"); all_passed = false; } if (Polygon_complete(OBC_boundary, ARRAY_SIZE(OBC_boundary)-1)) { hal.console->println("Polygon_complete test failed"); all_passed = false; } for (i=0; i<ARRAY_SIZE(test_points); i++) { bool result; result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_SIZE(OBC_boundary)); hal.console->printf_P(PSTR("%10f,%10f %s %s\n"), 1.0e-7f*test_points[i].point.x, 1.0e-7f*test_points[i].point.y, result ? "OUTSIDE" : "INSIDE ", result == test_points[i].outside ? "PASS" : "FAIL"); if (result != test_points[i].outside) { all_passed = false; } } hal.console->println(all_passed ? "TEST PASSED" : "TEST FAILED"); hal.console->println("Speed test:"); start_time = hal.scheduler->micros(); for (count=0; count<1000; count++) { for (i=0; i<ARRAY_SIZE(test_points); i++) { bool result; result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_SIZE(OBC_boundary)); if (result != test_points[i].outside) { all_passed = false; } } } hal.console->printf("%u usec/call\n", (unsigned)((hal.scheduler->micros() - start_time)/(count*ARRAY_SIZE(test_points)))); hal.console->println(all_passed ? "ALL TESTS PASSED" : "TEST FAILED"); }
bool AP_Limit_Geofence::boundary_correct() { if (Polygon_complete(&_boundary[1], _fence_total - 1) && !Polygon_outside(_boundary[0], &_boundary[1], _fence_total - 1)) { return true; } else return false; }
bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2<T>* points) const { // exit immediate if no points if (points == nullptr) { return false; } // start from 2nd point as boundary contains return point (as first point) uint8_t start_num = 1; // a boundary requires at least 4 point (a triangle and last point equals first) if (num_points < start_num + 4) { return false; } // point 1 and last point must be the same. Note: 0th point is reserved as the return point if (!Polygon_complete(&points[start_num], num_points-start_num)) { return false; } // check return point is within the fence if (Polygon_outside(points[0], &points[1], num_points-start_num)) { return false; } return true; }
/* * allocate and fill the geofence state structure */ void Plane::geofence_load(void) { uint8_t i; if (geofence_state == nullptr) { uint16_t boundary_size = sizeof(Vector2l) * max_fencepoints(); if (hal.util->available_memory() < 100 + boundary_size + sizeof(struct GeofenceState)) { // too risky to enable as we could run out of stack goto failed; } geofence_state = (struct GeofenceState *)calloc(1, sizeof(struct GeofenceState)); if (geofence_state == nullptr) { // not much we can do here except disable it goto failed; } geofence_state->boundary = (Vector2l *)calloc(1, boundary_size); if (geofence_state->boundary == nullptr) { free(geofence_state); geofence_state = nullptr; goto failed; } geofence_state->old_switch_position = 254; } if (g.fence_total <= 0) { g.fence_total.set(0); return; } for (i=0; i<g.fence_total; i++) { geofence_state->boundary[i] = get_fence_point_with_index(i); } geofence_state->num_points = i; if (!Polygon_complete(&geofence_state->boundary[1], geofence_state->num_points-1)) { // first point and last point must be the same goto failed; } if (Polygon_outside(geofence_state->boundary[0], &geofence_state->boundary[1], geofence_state->num_points-1)) { // return point needs to be inside the fence goto failed; } geofence_state->boundary_uptodate = true; geofence_state->fence_triggered = false; gcs_send_text(MAV_SEVERITY_INFO,"Geofence loaded"); gcs_send_message(MSG_FENCE_STATUS); return; failed: g.fence_action.set(FENCE_ACTION_NONE); gcs_send_text(MAV_SEVERITY_WARNING,"Geofence setup error"); }
/* * allocate and fill the geofence state structure */ void Plane::geofence_load(void) { uint8_t i; if (geofence_state == nullptr) { uint16_t boundary_size = sizeof(Vector2l) * max_fencepoints(); if (hal.util->available_memory() < 100 + boundary_size + sizeof(struct GeofenceState)) { // too risky to enable as we could run out of stack geofence_disable_and_send_error_msg("low on memory"); return; } geofence_state = (struct GeofenceState *)calloc(1, sizeof(struct GeofenceState)); if (geofence_state == nullptr) { // not much we can do here except disable it geofence_disable_and_send_error_msg("failed to init state memory"); return; } geofence_state->boundary = (Vector2l *)calloc(1, boundary_size); if (geofence_state->boundary == nullptr) { free(geofence_state); geofence_state = nullptr; geofence_disable_and_send_error_msg("failed to init boundary memory"); return; } geofence_state->old_switch_position = 254; } if (g.fence_total <= 0) { g.fence_total.set(0); return; } for (i=0; i<g.fence_total; i++) { geofence_state->boundary[i] = get_fence_point_with_index(i); } geofence_state->num_points = i; if (!Polygon_complete(&geofence_state->boundary[1], geofence_state->num_points-1)) { geofence_disable_and_send_error_msg("pt[1] and pt[total-1] must match"); return; } if (Polygon_outside(geofence_state->boundary[0], &geofence_state->boundary[1], geofence_state->num_points-1)) { geofence_disable_and_send_error_msg("pt[0] must be inside fence"); return; } geofence_state->boundary_uptodate = true; geofence_state->fence_triggered = false; gcs().send_text(MAV_SEVERITY_INFO,"Geofence loaded"); gcs().send_message(MSG_FENCE_STATUS); }