Plane: cleanup geofence warnings

ArduPlane/GCS_Mavlink.cpp

@@ -1448,10 +1448,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
         } else if (!check_latlng(packet.lat,packet.lng)) {
             send_text(MAV_SEVERITY_WARNING,"Invalid fence point, lat or lng too large");
         } else {
-            Vector2l point;
+            plane.set_fence_point_with_index(Vector2l(packet.lat*1.0e7f, packet.lng*1.0e7f), packet.idx);
-            point.x = packet.lat*1.0e7f;
-            point.y = packet.lng*1.0e7f;
-            plane.set_fence_point_with_index(point, packet.idx);
         }
         break;
     }

ArduPlane/Plane.h

@@ -906,7 +906,7 @@ private:
     void update_events(void);
     uint8_t max_fencepoints(void);
     Vector2l get_fence_point_with_index(unsigned i);
-    void set_fence_point_with_index(Vector2l &point, unsigned i);
+    void set_fence_point_with_index(const Vector2l &point, unsigned i);
     void geofence_load(void);
     bool geofence_present(void);
     void geofence_update_pwm_enabled_state();
@@ -919,6 +919,7 @@ private:
     bool geofence_stickmixing(void);
     void geofence_send_status(mavlink_channel_t chan);
     bool geofence_breached(void);
+    void geofence_disable_and_send_error_msg(const char *errorMsg);
     void disarm_if_autoland_complete();
     float tecs_hgt_afe(void);
     void set_nav_controller(void);

ArduPlane/geofence.cpp

@@ -7,9 +7,9 @@
 
 #if GEOFENCE_ENABLED == ENABLED
 
-#define MIN_GEOFENCE_POINTS 5 // 3 to define a minimal polygon (triangle)
+#define MIN_GEOFENCE_POINTS 5 // index [0] for return point, must be inside polygon
-                              // + 1 for return point and +1 for last
+                              // index [1 to n-1] to define a polygon, minimum 3 for a triangle
-                              // pt (same as first)
+                              // index [n] (must be same as index 1 to close the polygon)
 
 /*
  *  The state of geo-fencing. This structure is dynamically allocated
@@ -64,13 +64,13 @@ Vector2l Plane::get_fence_point_with_index(unsigned i)
 
     // read fence point
     ret.x = fence_storage.read_uint32(i * sizeof(Vector2l));
-    ret.y = fence_storage.read_uint32(i * sizeof(Vector2l) + 4);
+    ret.y = fence_storage.read_uint32(i * sizeof(Vector2l) + sizeof(int32_t));
 
     return ret;
 }
 
 // save a fence point
-void Plane::set_fence_point_with_index(Vector2l &point, unsigned i)
+void Plane::set_fence_point_with_index(const Vector2l &point, unsigned i)
 {
     if (i >= (unsigned)g.fence_total.get() || i >= max_fencepoints()) {
         // not allowed
@@ -78,7 +78,7 @@ void Plane::set_fence_point_with_index(Vector2l &point, unsigned i)
     }
 
     fence_storage.write_uint32(i * sizeof(Vector2l), point.x);
-    fence_storage.write_uint32(i * sizeof(Vector2l)+4, point.y);
+    fence_storage.write_uint32(i * sizeof(Vector2l) + sizeof(int32_t), point.y);
 
     if (geofence_state != nullptr) {
         geofence_state->boundary_uptodate = false;
@@ -96,19 +96,22 @@ void Plane::geofence_load(void)
         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_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
-            goto failed;
+            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;
-            goto failed;
+            geofence_disable_and_send_error_msg("failed to init boundary memory");
+            return;
         }
         
         geofence_state->old_switch_position = 254;
@@ -125,12 +128,12 @@ void Plane::geofence_load(void)
     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
+        geofence_disable_and_send_error_msg("pt[1] and pt[total-1] must match");
-        goto failed;
+        return;
     }
     if (Polygon_outside(geofence_state->boundary[0], &geofence_state->boundary[1], geofence_state->num_points-1)) {
-        // return point needs to be inside the fence
+        geofence_disable_and_send_error_msg("pt[0] must be inside fence");
-        goto failed;
+        return;
     }
 
     geofence_state->boundary_uptodate = true;
@@ -138,11 +141,15 @@ void Plane::geofence_load(void)
 
     gcs().send_text(MAV_SEVERITY_INFO,"Geofence loaded");
     gcs().send_message(MSG_FENCE_STATUS);
-    return;
+}
 
-failed:
+/*
+ *  Disable geofence and send an error message string
+ */
+void Plane::geofence_disable_and_send_error_msg(const char *errorMsg)
+{
     g.fence_action.set(FENCE_ACTION_NONE);
-    gcs().send_text(MAV_SEVERITY_WARNING,"Geofence setup error");
+    gcs().send_text(MAV_SEVERITY_WARNING,"Geofence error, %s", errorMsg);
 }
 
 /*