zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

geofence: store fence points as int32_t 

this keeps maximum precision in fence boundaries
master
Andrew Tridgell 13 years ago
parent
fc495ce6a7
commit
42522baf9f
6 changed files with 77 additions and 59 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 15
      ArduPlane/GCS_Mavlink.pde
  2. 2
      ArduPlane/defines.h
  3. 34
      ArduPlane/geofence.pde
  4. 57
      libraries/AP_Math/examples/polygon/polygon.pde
  5. 24
      libraries/AP_Math/polygon.cpp
  6. 4
      libraries/AP_Math/polygon.h

15
ArduPlane/GCS_Mavlink.pde
Unescape View File

@ -1651,17 +1651,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -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) @@ -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;
}

2
ArduPlane/defines.h
Unescape View File

@ -216,7 +216,7 @@ enum gcs_severity { @@ -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

34
ArduPlane/geofence.pde
Unescape View File

@ -23,33 +23,33 @@ static struct geofence_state { @@ -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) @@ -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) @@ -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) @@ -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) @@ -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

57
libraries/AP_Math/examples/polygon/polygon.pde
Unescape View File

@ -14,33 +14,35 @@ FastSerialPort(Serial, 0); @@ -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 { @@ -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) @@ -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) {

24
libraries/AP_Math/polygon.cpp
Unescape View File

@ -30,15 +30,29 @@ @@ -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) @@ -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);
}

4
libraries/AP_Math/polygon.h
Unescape View File

@ -17,6 +17,6 @@ @@ -17,6 +17,6 @@
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
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);

Write Preview
Loading…
Cancel
Save