diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index eb82e0d1a9..762f2f110e 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1651,17 +1651,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // receive a fence point from GCS and store in EEPROM case MAVLINK_MSG_ID_FENCE_POINT: { mavlink_fence_point_t packet; + mavlink_msg_fence_point_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; - mavlink_msg_fence_point_decode(msg, &packet); if (g.fence_action != FENCE_ACTION_NONE) { send_text(SEVERITY_LOW,PSTR("fencing must be disabled")); } else if (packet.count != g.fence_total) { send_text(SEVERITY_LOW,PSTR("bad fence point")); } else { - Vector2f point; - point.x = packet.lat; - point.y = packet.lng; + Vector2l point; + point.x = packet.lat*1.0e7; + point.y = packet.lng*1.0e7; set_fence_point_with_index(point, packet.idx); } break; @@ -1670,14 +1670,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // send a fence point to GCS case MAVLINK_MSG_ID_FENCE_FETCH_POINT: { mavlink_fence_fetch_point_t packet; + mavlink_msg_fence_fetch_point_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; - mavlink_msg_fence_fetch_point_decode(msg, &packet); if (packet.idx >= g.fence_total) { send_text(SEVERITY_LOW,PSTR("bad fence point")); } else { - Vector2f point = get_fence_point_with_index(packet.idx); - mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, g.fence_total, point.x, point.y); + Vector2l point = get_fence_point_with_index(packet.idx); + mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, g.fence_total, + point.x*1.0e-7, point.y*1.0e-7); } break; } diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 56b4067434..3c365f68eb 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -216,7 +216,7 @@ enum gcs_severity { // fence points are stored at the end of the EEPROM #define MAX_FENCEPOINTS 20 -#define FENCE_WP_SIZE sizeof(Vector2f) +#define FENCE_WP_SIZE sizeof(Vector2l) #define FENCE_START_BYTE (EEPROM_MAX_ADDR-(MAX_FENCEPOINTS*FENCE_WP_SIZE)) #define MAX_WAYPOINTS ((FENCE_START_BYTE - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde index 4185dbd113..c623ce05d9 100644 --- a/ArduPlane/geofence.pde +++ b/ArduPlane/geofence.pde @@ -23,33 +23,33 @@ static struct geofence_state { uint8_t breach_type; uint32_t breach_time; /* point 0 is the return point */ - Vector2f boundary[MAX_FENCEPOINTS]; + Vector2l boundary[MAX_FENCEPOINTS]; } *geofence_state; /* fence boundaries fetch/store */ -static Vector2f get_fence_point_with_index(unsigned i) +static Vector2l get_fence_point_with_index(unsigned i) { uint32_t mem; - Vector2f ret; + Vector2l ret; if (i > (unsigned)g.fence_total) { - return Vector2f(0,0); + return Vector2l(0,0); } // read fence point mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE); - eeprom_read_block(&ret.x, (void *)mem, sizeof(float)); - mem += sizeof(float); - eeprom_read_block(&ret.y, (void *)mem, sizeof(float)); + ret.x = eeprom_read_dword((uint32_t *)mem); + mem += sizeof(uint32_t); + ret.y = eeprom_read_dword((uint32_t *)mem); return ret; } // save a fence point -static void set_fence_point_with_index(Vector2f &point, unsigned i) +static void set_fence_point_with_index(Vector2l &point, unsigned i) { uint32_t mem; @@ -60,9 +60,9 @@ static void set_fence_point_with_index(Vector2f &point, unsigned i) mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE); - eeprom_write_block(&point.x, (void *)mem, sizeof(float)); - mem += 4; - eeprom_write_block(&point.y, (void *)mem, sizeof(float)); + eeprom_write_dword((uint32_t *)mem, point.x); + mem += sizeof(uint32_t); + eeprom_write_dword((uint32_t *)mem, point.y); if (geofence_state != NULL) { geofence_state->boundary_uptodate = false; @@ -77,7 +77,7 @@ static void geofence_load(void) uint8_t i; if (geofence_state == NULL) { - if (memcheck_available_memory() < 1024 + sizeof(struct geofence_state)) { + if (memcheck_available_memory() < 512 + sizeof(struct geofence_state)) { // too risky to enable as we could run out of stack goto failed; } @@ -195,9 +195,9 @@ static void geofence_check(bool altitude_check_only) outside = true; breach_type = FENCE_BREACH_MAXALT; } else if (!altitude_check_only) { - Vector2f location; - location.x = 1.0e-7 * current_loc.lat; - location.y = 1.0e-7 * current_loc.lng; + Vector2l location; + location.x = current_loc.lat; + location.y = current_loc.lng; outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1); if (outside) { breach_type = FENCE_BREACH_BOUNDARY; @@ -243,8 +243,8 @@ static void geofence_check(bool altitude_check_only) guided_WP.id = 0; guided_WP.p1 = 0; guided_WP.options = 0; - guided_WP.lat = geofence_state->boundary[0].x * 1.0e7; - guided_WP.lng = geofence_state->boundary[0].y * 1.0e7; + guided_WP.lat = geofence_state->boundary[0].x; + guided_WP.lng = geofence_state->boundary[0].y; if (control_mode == MANUAL && g.auto_trim) { // make sure we don't auto trim the surfaces on this change diff --git a/libraries/AP_Math/examples/polygon/polygon.pde b/libraries/AP_Math/examples/polygon/polygon.pde index d892055a9c..a438ed1368 100644 --- a/libraries/AP_Math/examples/polygon/polygon.pde +++ b/libraries/AP_Math/examples/polygon/polygon.pde @@ -14,33 +14,35 @@ FastSerialPort(Serial, 0); Note that the last point must be the same as the first for the Polygon_outside() algorithm */ -static const Vector2f OBC_boundary[] = { - Vector2f(-26.569564000, 151.837373000), - Vector2f(-26.569956000, 151.839405000), - Vector2f(-26.576823000, 151.841142000), - Vector2f(-26.577308000, 151.840344000), - Vector2f(-26.581511000, 151.841950000), - Vector2f(-26.578486000, 151.847469000), - Vector2f(-26.599489000, 151.852886000), - Vector2f(-26.609211000, 151.874742000), - Vector2f(-26.645478000, 151.882053000), - Vector2f(-26.643572000, 151.830350000), - Vector2f(-26.587599000, 151.834405000), - Vector2f(-26.569564000, 151.837373000) +static const Vector2l OBC_boundary[] = { + Vector2l(-265695640, 1518373730), + Vector2l(-265699560, 1518394050), + Vector2l(-265768230, 1518411420), + Vector2l(-265773080, 1518403440), + Vector2l(-265815110, 1518419500), + Vector2l(-265784860, 1518474690), + Vector2l(-265994890, 1518528860), + Vector2l(-266092110, 1518747420), + Vector2l(-266454780, 1518820530), + Vector2l(-266435720, 1518303500), + Vector2l(-265875990, 1518344050), + Vector2l(-265695640, 1518373730) }; static const struct { - Vector2f point; - bool outside; + Vector2l point; + bool outside; } test_points[] = { - { Vector2f(-26.639887, 151.822000), true }, - { Vector2f(-26.641870, 151.870926), false }, - { Vector2f(-35.0, 149.0), true }, - { Vector2f(0, 0), true }, - { Vector2f(-26.576815, 151.840825), false }, - { Vector2f(-26.577406, 151.840586), true }, - { Vector2f(-26.643563, 151.830344), true }, - { Vector2f(-26.643565, 151.831354), false }, + { Vector2l(-266398870, 1518220000), true }, + { Vector2l(-266418700, 1518709260), false }, + { Vector2l(-350000000, 1490000000), true }, + { Vector2l(0, 0), true }, + { Vector2l(-265768150, 1518408250), false }, + { Vector2l(-265774060, 1518405860), true }, + { Vector2l(-266435630, 1518303440), true }, + { Vector2l(-266435650, 1518313540), false }, + { Vector2l(-266435690, 1518303530), false }, + { Vector2l(-266435690, 1518303490), true }, }; #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) @@ -50,12 +52,12 @@ static const struct { */ void setup(void) { - unsigned i, count; + unsigned i, count; bool all_passed = true; uint32_t start_time; - Serial.begin(115200); - Serial.println("polygon unit tests\n"); + Serial.begin(115200); + Serial.println("polygon unit tests\n"); if (!Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary))) { Serial.println("OBC boundary is not complete!"); @@ -71,7 +73,8 @@ void setup(void) bool result; result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary)); Serial.printf_P(PSTR("%10f,%10f %s %s\n"), - test_points[i].point.x, test_points[i].point.y, + 1.0e-7*test_points[i].point.x, + 1.0e-7*test_points[i].point.y, result?"OUTSIDE":"INSIDE ", result == test_points[i].outside?"PASS":"FAIL"); if (result != test_points[i].outside) { diff --git a/libraries/AP_Math/polygon.cpp b/libraries/AP_Math/polygon.cpp index 59416948db..f3070a8838 100644 --- a/libraries/AP_Math/polygon.cpp +++ b/libraries/AP_Math/polygon.cpp @@ -30,15 +30,29 @@ Input: P = a point, V[] = vertex points of a polygon V[n+1] with V[n]=V[0] Return: true if P is outside the polygon + + This does not take account of the curvature of the earth, but we + expect that to be very small over the distances involved in the + fence boundary */ -bool Polygon_outside(const Vector2f &P, const Vector2f *V, unsigned n) +bool Polygon_outside(const Vector2l &P, const Vector2l *V, unsigned n) { unsigned i, j; bool outside = true; for (i = 0, j = n-1; i < n; j = i++) { - if ( ((V[i].y > P.y) != (V[j].y > P.y)) && - (P.x < (V[j].x - V[i].x) * (P.y - V[i].y) / (V[j].y - V[i].y) + V[i].x) ) - outside = !outside; + if ((V[i].y > P.y) == (V[j].y > P.y)) { + continue; + } + float dx1, dx2, dy1, dy2; + // we convert the deltas to floating point numbers to + // prevent integer overflow while maintaining maximum precision + dx1 = P.x - V[i].x; + dx2 = V[j].x - V[i].x; + dy1 = P.y - V[i].y; + dy2 = V[j].y - V[i].y; + if ( dx1 < dx2 * dy1 / dy2 ) { + outside = !outside; + } } return outside; } @@ -50,7 +64,7 @@ bool Polygon_outside(const Vector2f &P, const Vector2f *V, unsigned n) and the first point is the same as the last point. That is the minimum requirement for the Polygon_outside function to work */ -bool Polygon_complete(const Vector2f *V, unsigned n) +bool Polygon_complete(const Vector2l *V, unsigned n) { return (n >= 4 && V[n-1].x == V[0].x && V[n-1].y == V[0].y); } diff --git a/libraries/AP_Math/polygon.h b/libraries/AP_Math/polygon.h index 56370e86f6..ae36ee3a63 100644 --- a/libraries/AP_Math/polygon.h +++ b/libraries/AP_Math/polygon.h @@ -17,6 +17,6 @@ * with this program. If not, see . */ -bool Polygon_outside(const Vector2f &P, const Vector2f *V, unsigned n); -bool Polygon_complete(const Vector2f *V, unsigned n); +bool Polygon_outside(const Vector2l &P, const Vector2l *V, unsigned n); +bool Polygon_complete(const Vector2l *V, unsigned n);