Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

geofence: store fence points as int32_t

Browse Source 
this keeps maximum precision in fence boundaries
This commit is contained in:
Andrew Tridgell 2011-12-16 13:48:15 +11:00
parent fc495ce6a7
commit 42522baf9f
6 changed files with 77 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduPlane/GCS_Mavlink.pde
View File

@ -1651,17 +1651,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// receive a fence point from GCS and store in EEPROM // receive a fence point from GCS and store in EEPROM
case MAVLINK_MSG_ID_FENCE_POINT: { case MAVLINK_MSG_ID_FENCE_POINT: {
mavlink_fence_point_t packet; mavlink_fence_point_t packet;
mavlink_msg_fence_point_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) if (mavlink_check_target(packet.target_system, packet.target_component))
break; break;
mavlink_msg_fence_point_decode(msg, &packet);
if (g.fence_action != FENCE_ACTION_NONE) { if (g.fence_action != FENCE_ACTION_NONE) {
send_text(SEVERITY_LOW,PSTR("fencing must be disabled")); send_text(SEVERITY_LOW,PSTR("fencing must be disabled"));
} else if (packet.count != g.fence_total) { } else if (packet.count != g.fence_total) {
send_text(SEVERITY_LOW,PSTR("bad fence point")); send_text(SEVERITY_LOW,PSTR("bad fence point"));
} else { } else {
Vector2f point; Vector2l point;
point.x = packet.lat; point.x = packet.lat*1.0e7;
point.y = packet.lng; point.y = packet.lng*1.0e7;
set_fence_point_with_index(point, packet.idx); set_fence_point_with_index(point, packet.idx);
} }
break; break;
@ -1670,14 +1670,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// send a fence point to GCS // send a fence point to GCS
case MAVLINK_MSG_ID_FENCE_FETCH_POINT: { case MAVLINK_MSG_ID_FENCE_FETCH_POINT: {
mavlink_fence_fetch_point_t packet; mavlink_fence_fetch_point_t packet;
mavlink_msg_fence_fetch_point_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) if (mavlink_check_target(packet.target_system, packet.target_component))
break; break;
mavlink_msg_fence_fetch_point_decode(msg, &packet);
if (packet.idx >= g.fence_total) { if (packet.idx >= g.fence_total) {
send_text(SEVERITY_LOW,PSTR("bad fence point")); send_text(SEVERITY_LOW,PSTR("bad fence point"));
} else { } else {
Vector2f point = get_fence_point_with_index(packet.idx); Vector2l 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); mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, g.fence_total,
point.x*1.0e-7, point.y*1.0e-7);
} }
break; break;
} }

2
ArduPlane/defines.h
View File

@ -216,7 +216,7 @@ enum gcs_severity {
// fence points are stored at the end of the EEPROM // fence points are stored at the end of the EEPROM
#define MAX_FENCEPOINTS 20 #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 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 #define MAX_WAYPOINTS ((FENCE_START_BYTE - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe

34
ArduPlane/geofence.pde
View File

@ -23,33 +23,33 @@ static struct geofence_state {
uint8_t breach_type; uint8_t breach_type;
uint32_t breach_time; uint32_t breach_time;
/* point 0 is the return point */ /* point 0 is the return point */
Vector2f boundary[MAX_FENCEPOINTS]; Vector2l boundary[MAX_FENCEPOINTS];
} *geofence_state; } *geofence_state;
/* /*
fence boundaries fetch/store 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; uint32_t mem;
Vector2f ret; Vector2l ret;
if (i > (unsigned)g.fence_total) { if (i > (unsigned)g.fence_total) {
return Vector2f(0,0); return Vector2l(0,0);
} }
// read fence point // read fence point
mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE); mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE);
eeprom_read_block(&ret.x, (void *)mem, sizeof(float)); ret.x = eeprom_read_dword((uint32_t *)mem);
mem += sizeof(float); mem += sizeof(uint32_t);
eeprom_read_block(&ret.y, (void *)mem, sizeof(float)); ret.y = eeprom_read_dword((uint32_t *)mem);
return ret; return ret;
} }
// save a fence point // 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; 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); mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE);
eeprom_write_block(&point.x, (void *)mem, sizeof(float)); eeprom_write_dword((uint32_t *)mem, point.x);
mem += 4; mem += sizeof(uint32_t);
eeprom_write_block(&point.y, (void *)mem, sizeof(float)); eeprom_write_dword((uint32_t *)mem, point.y);
if (geofence_state != NULL) { if (geofence_state != NULL) {
geofence_state->boundary_uptodate = false; geofence_state->boundary_uptodate = false;
@ -77,7 +77,7 @@ static void geofence_load(void)
uint8_t i; uint8_t i;
if (geofence_state == NULL) { 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 // too risky to enable as we could run out of stack
goto failed; goto failed;
} }
@ -195,9 +195,9 @@ static void geofence_check(bool altitude_check_only)
outside = true; outside = true;
breach_type = FENCE_BREACH_MAXALT; breach_type = FENCE_BREACH_MAXALT;
} else if (!altitude_check_only) { } else if (!altitude_check_only) {
Vector2f location; Vector2l location;
location.x = 1.0e-7 * current_loc.lat; location.x = current_loc.lat;
location.y = 1.0e-7 * current_loc.lng; location.y = current_loc.lng;
outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1); outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
if (outside) { if (outside) {
breach_type = FENCE_BREACH_BOUNDARY; breach_type = FENCE_BREACH_BOUNDARY;
@ -243,8 +243,8 @@ static void geofence_check(bool altitude_check_only)
guided_WP.id = 0; guided_WP.id = 0;
guided_WP.p1 = 0; guided_WP.p1 = 0;
guided_WP.options = 0; guided_WP.options = 0;
guided_WP.lat = geofence_state->boundary[0].x * 1.0e7; guided_WP.lat = geofence_state->boundary[0].x;
guided_WP.lng = geofence_state->boundary[0].y * 1.0e7; guided_WP.lng = geofence_state->boundary[0].y;
if (control_mode == MANUAL && g.auto_trim) { if (control_mode == MANUAL && g.auto_trim) {
// make sure we don't auto trim the surfaces on this change // make sure we don't auto trim the surfaces on this change

49
libraries/AP_Math/examples/polygon/polygon.pde
View File

@ -14,33 +14,35 @@ FastSerialPort(Serial, 0);
Note that the last point must be the same as the first for the Note that the last point must be the same as the first for the
Polygon_outside() algorithm Polygon_outside() algorithm
*/ */
static const Vector2f OBC_boundary[] = { static const Vector2l OBC_boundary[] = {
Vector2f(-26.569564000, 151.837373000), Vector2l(-265695640, 1518373730),
Vector2f(-26.569956000, 151.839405000), Vector2l(-265699560, 1518394050),
Vector2f(-26.576823000, 151.841142000), Vector2l(-265768230, 1518411420),
Vector2f(-26.577308000, 151.840344000), Vector2l(-265773080, 1518403440),
Vector2f(-26.581511000, 151.841950000), Vector2l(-265815110, 1518419500),
Vector2f(-26.578486000, 151.847469000), Vector2l(-265784860, 1518474690),
Vector2f(-26.599489000, 151.852886000), Vector2l(-265994890, 1518528860),
Vector2f(-26.609211000, 151.874742000), Vector2l(-266092110, 1518747420),
Vector2f(-26.645478000, 151.882053000), Vector2l(-266454780, 1518820530),
Vector2f(-26.643572000, 151.830350000), Vector2l(-266435720, 1518303500),
Vector2f(-26.587599000, 151.834405000), Vector2l(-265875990, 1518344050),
Vector2f(-26.569564000, 151.837373000) Vector2l(-265695640, 1518373730)
}; };
static const struct { static const struct {
Vector2f point; Vector2l point;
bool outside; bool outside;
} test_points[] = { } test_points[] = {
{ Vector2f(-26.639887, 151.822000), true }, { Vector2l(-266398870, 1518220000), true },
{ Vector2f(-26.641870, 151.870926), false }, { Vector2l(-266418700, 1518709260), false },
{ Vector2f(-35.0, 149.0), true }, { Vector2l(-350000000, 1490000000), true },
{ Vector2f(0, 0), true }, { Vector2l(0, 0), true },
{ Vector2f(-26.576815, 151.840825), false }, { Vector2l(-265768150, 1518408250), false },
{ Vector2f(-26.577406, 151.840586), true }, { Vector2l(-265774060, 1518405860), true },
{ Vector2f(-26.643563, 151.830344), true }, { Vector2l(-266435630, 1518303440), true },
{ Vector2f(-26.643565, 151.831354), false }, { Vector2l(-266435650, 1518313540), false },
{ Vector2l(-266435690, 1518303530), false },
{ Vector2l(-266435690, 1518303490), true },
}; };
#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
@ -71,7 +73,8 @@ void setup(void)
bool result; bool result;
result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary)); result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary));
Serial.printf_P(PSTR("%10f,%10f %s %s\n"), 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?"OUTSIDE":"INSIDE ",
result == test_points[i].outside?"PASS":"FAIL"); result == test_points[i].outside?"PASS":"FAIL");
if (result != test_points[i].outside) { if (result != test_points[i].outside) {

22
libraries/AP_Math/polygon.cpp
View File

@ -30,16 +30,30 @@
Input: P = a point, Input: P = a point,
V[] = vertex points of a polygon V[n+1] with V[n]=V[0] V[] = vertex points of a polygon V[n+1] with V[n]=V[0]
Return: true if P is outside the polygon 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; unsigned i, j;
bool outside = true; bool outside = true;
for (i = 0, j = n-1; i < n; j = i++) { for (i = 0, j = n-1; i < n; j = i++) {
if ( ((V[i].y > P.y) != (V[j].y > P.y)) && 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) ) 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; outside = !outside;
} }
}
return 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 and the first point is the same as the last point. That is the
minimum requirement for the Polygon_outside function to work 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); return (n >= 4 && V[n-1].x == V[0].x && V[n-1].y == V[0].y);
} }

4
libraries/AP_Math/polygon.h
View File

@ -17,6 +17,6 @@
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
bool Polygon_outside(const Vector2f &P, const Vector2f *V, unsigned n); bool Polygon_outside(const Vector2l &P, const Vector2l *V, unsigned n);
bool Polygon_complete(const Vector2f *V, unsigned n); bool Polygon_complete(const Vector2l *V, unsigned n);