diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 3c70b4dd3e..17323e4420 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1288,16 +1288,20 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (!geofence_present()) { result = MAV_RESULT_FAILED; - } else { - if (packet.param1 == 0) { + } switch((uint16_t)packet.param1) { + case 0: if (! geofence_set_enabled(false, GCS_TOGGLED)) { result = MAV_RESULT_FAILED; } - } else { + break; + case 1: if (! geofence_set_enabled(true, GCS_TOGGLED)) { result = MAV_RESULT_FAILED; } - } + break; + default: + result = MAV_RESULT_FAILED; + break; } break; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 2e86f3707d..c79a717d5b 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -275,14 +275,14 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: FENCE_AUTOENABLE // @DisplayName: Fence automatically enabled after auto takeoff and automatically disabled when starting an auto landing. Note that this does NOT remove the need to first create a geofence. // @Description: When set to 1, gefence automatically enables after an auto takeoff and automatically disables at the beginning of an auto landing. When on the ground before takeoff the fence is disabled. - // @Range: 0 1 + // @Values: 0:NoAutoEnable,1:AutoEnable // @User: Standard GSCALAR(fence_autoenable, "FENCE_AUTOENABLE", 0), // @Param: FENCE_RET_RALLY // @DisplayName: Fence Return to Rally // @Description: When set to 1: on fence breach the plane will return to the nearest rally point rather than the fence return point. If no rally points have been defined the plane will return to the home point. - // @Range: 0 1 + // @Values: 0:FenceReturnPoint,1:NearestRallyPoint // @User: Standard GSCALAR(fence_ret_rally, "FENCE_RET_RALLY", 0), #endif diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde index 3bb47d6a5a..7edd289915 100644 --- a/ArduPlane/geofence.pde +++ b/ArduPlane/geofence.pde @@ -5,6 +5,10 @@ */ #if GEOFENCE_ENABLED == ENABLED + +#define MIN_GEOFENCE_POINTS 4 //3 to define a minimal polygon (triangle) + //+ 1 for return point. + /* * The state of geo-fencing. This structure is dynamically allocated * the first time it is used. This means we only pay for the pointer @@ -126,13 +130,14 @@ failed: } /* - * return true if a geo-fence has been uploaded (not necessarily enabled) + * return true if a geo-fence has been uploaded and + * FENCE_ACTION is 1 (not necessarily enabled) */ static bool geofence_present(void) { //require at least a return point and a triangle //to define a geofence area: - if (g.fence_total < 4) { + if (g.fence_action != FENCE_ACTION_NONE && g.fence_total < MIN_GEOFENCE_POINTS) { return false; } return true;