From 3d6cc59a6b8d8f873ace38c63aab2e14e8a4181c Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 21 Nov 2017 15:49:12 -0800 Subject: [PATCH] Plane: cleanup geofence warnings --- ArduPlane/GCS_Mavlink.cpp | 5 +---- ArduPlane/Plane.h | 3 ++- ArduPlane/geofence.cpp | 39 +++++++++++++++++++++++---------------- 3 files changed, 26 insertions(+), 21 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 93d7f3dbcb..29db30f691 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/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; - point.x = packet.lat*1.0e7f; - point.y = packet.lng*1.0e7f; - plane.set_fence_point_with_index(point, packet.idx); + plane.set_fence_point_with_index(Vector2l(packet.lat*1.0e7f, packet.lng*1.0e7f), packet.idx); } break; } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1d25ad0140..935de42c2d 100644 --- a/ArduPlane/Plane.h +++ b/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); diff --git a/ArduPlane/geofence.cpp b/ArduPlane/geofence.cpp index 7c1ca7422d..54eb674920 100644 --- a/ArduPlane/geofence.cpp +++ b/ArduPlane/geofence.cpp @@ -7,9 +7,9 @@ #if GEOFENCE_ENABLED == ENABLED -#define MIN_GEOFENCE_POINTS 5 // 3 to define a minimal polygon (triangle) - // + 1 for return point and +1 for last - // pt (same as first) +#define MIN_GEOFENCE_POINTS 5 // index [0] for return point, must be inside polygon + // index [1 to n-1] to define a polygon, minimum 3 for a triangle + // 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 - goto failed; + 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)) { - // return point needs to be inside the fence - goto failed; + geofence_disable_and_send_error_msg("pt[0] must be inside fence"); + 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); } /*