/*
* 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;
}
bool AP_Limit_Geofence::triggered() {
// reset trigger before checking
_triggered = false;
// never trigger while disabled
if (!_enabled) return false;
// if Geofence is required and we don't know where we are, trigger.
if (_required && (!_gps || !_gps->fix || !_home || !_current_loc)) {
// TRIGGER
_triggered = true;
}
uint32_t distance;
// simple mode, pointers to current and home exist.
if (_simple && _current_loc && _home) {
distance = (uint32_t) get_distance(*_current_loc, *_home);
if (distance > 0 && distance > (uint16_t) _radius) {
// TRIGGER
_triggered = true;
}
}
else {
// COMPLEX GEOFENCE mode
// check boundary and update if necessary
if (!_boundary_uptodate) {
update_boundary();
}
// if boundary is correct, and current_loc exists check if we
// are inside the fence.
if (boundary_correct() && _current_loc) {
Vector2l location;
location.x = _current_loc->lat;
location.y = _current_loc->lng;
// trigger if outside
if (Polygon_outside(location, &_boundary[1], _fence_total-1)) {
// TRIGGER
_triggered = true;
}
} else {
// boundary incorrect
// If geofence is required and our boundary fence is incorrect,
// we trigger.
if (_required) {
// TRIGGER
_triggered = true;
}
}
}
return _triggered;
}
/*
* 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);
}
bool AC_PolyFence_loader::boundary_breached(const Vector2<T>& location, 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;
// check location is within the fence
return Polygon_outside(location, &points[start_num], num_points-start_num);
}
/*
* check if we have breached the geo-fence
*/
void Plane::geofence_check(bool altitude_check_only)
{
if (!geofence_enabled()) {
// switch back to the chosen control mode if still in
// GUIDED to the return point
if (geofence_state != nullptr &&
(g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS || g.fence_action == FENCE_ACTION_RTL) &&
(control_mode == GUIDED || control_mode == AVOID_ADSB) &&
geofence_present() &&
geofence_state->boundary_uptodate &&
geofence_state->old_switch_position == oldSwitchPosition &&
guided_WP_loc.lat == geofence_state->guided_lat &&
guided_WP_loc.lng == geofence_state->guided_lng) {
geofence_state->old_switch_position = 254;
set_mode(get_previous_mode(), MODE_REASON_GCS_COMMAND);
}
return;
}
/* allocate the geo-fence state if need be */
if (geofence_state == nullptr || !geofence_state->boundary_uptodate) {
geofence_load();
if (!geofence_enabled()) {
// may have been disabled by load
return;
}
}
bool outside = false;
uint8_t breach_type = FENCE_BREACH_NONE;
struct Location loc;
// Never trigger a fence breach in the final stage of landing
if (landing.is_expecting_impact()) {
return;
}
if (geofence_state->floor_enabled && geofence_check_minalt()) {
outside = true;
breach_type = FENCE_BREACH_MINALT;
} else if (geofence_check_maxalt()) {
outside = true;
breach_type = FENCE_BREACH_MAXALT;
} else if (!altitude_check_only && ahrs.get_position(loc)) {
Vector2l location;
location.x = loc.lat;
location.y = loc.lng;
outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
if (outside) {
breach_type = FENCE_BREACH_BOUNDARY;
}
}
if (!outside) {
if (geofence_state->fence_triggered && !altitude_check_only) {
// we have moved back inside the fence
geofence_state->fence_triggered = false;
gcs_send_text(MAV_SEVERITY_INFO,"Geofence OK");
#if FENCE_TRIGGERED_PIN > 0
hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->write(FENCE_TRIGGERED_PIN, 0);
#endif
gcs_send_message(MSG_FENCE_STATUS);
}
// we're inside, all is good with the world
return;
}
// we are outside the fence
if (geofence_state->fence_triggered &&
(control_mode == GUIDED || control_mode == AVOID_ADSB || control_mode == RTL || g.fence_action == FENCE_ACTION_REPORT)) {
// we have already triggered, don't trigger again until the
// user disables/re-enables using the fence channel switch
return;
}
// we are outside, and have not previously triggered.
geofence_state->fence_triggered = true;
geofence_state->breach_count++;
geofence_state->breach_time = millis();
geofence_state->breach_type = breach_type;
#if FENCE_TRIGGERED_PIN > 0
hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->write(FENCE_TRIGGERED_PIN, 1);
#endif
gcs_send_text(MAV_SEVERITY_NOTICE,"Geofence triggered");
gcs_send_message(MSG_FENCE_STATUS);
// see what action the user wants
switch (g.fence_action) {
case FENCE_ACTION_REPORT:
break;
case FENCE_ACTION_GUIDED:
case FENCE_ACTION_GUIDED_THR_PASS:
case FENCE_ACTION_RTL:
// make sure we don't auto trim the surfaces on this mode change
int8_t saved_auto_trim = g.auto_trim;
g.auto_trim.set(0);
if (g.fence_action == FENCE_ACTION_RTL) {
set_mode(RTL, MODE_REASON_FENCE_BREACH);
} else {
set_mode(GUIDED, MODE_REASON_FENCE_BREACH);
}
g.auto_trim.set(saved_auto_trim);
if (g.fence_ret_rally != 0 || g.fence_action == FENCE_ACTION_RTL) { //return to a rally point
guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
} else { //return to fence return point, not a rally point
if (g.fence_retalt > 0) {
//fly to the return point using fence_retalt
guided_WP_loc.alt = home.alt + 100.0f*g.fence_retalt;
} else if (g.fence_minalt >= g.fence_maxalt) {
// invalid min/max, use RTL_altitude
guided_WP_loc.alt = home.alt + g.RTL_altitude_cm;
} else {
// fly to the return point, with an altitude half way between
// min and max
guided_WP_loc.alt = home.alt + 100.0f*(g.fence_minalt + g.fence_maxalt)/2;
}
guided_WP_loc.options = 0;
guided_WP_loc.lat = geofence_state->boundary[0].x;
guided_WP_loc.lng = geofence_state->boundary[0].y;
}
geofence_state->guided_lat = guided_WP_loc.lat;
geofence_state->guided_lng = guided_WP_loc.lng;
geofence_state->old_switch_position = oldSwitchPosition;
if (g.fence_action != FENCE_ACTION_RTL) { //not needed for RTL mode
setup_terrain_target_alt(guided_WP_loc);
set_guided_WP();
}
if (g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) {
guided_throttle_passthru = true;
}
break;
}
}