/*
* 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);
}
void AP_Limit_Geofence::update_boundary() {
if (!_simple && _fence_total > 0) {
for (uint8_t i = 0; i < (uint8_t) _fence_total; i++) {
_boundary[i] = get_fence_point_with_index(i);
}
_boundary_uptodate = true;
}
}
