Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -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
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;
}

2
ArduPlane/defines.h
View File

@ -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
View File

@ -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

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
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;
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]))
@ -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) {

22
libraries/AP_Math/polygon.cpp
View File

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

4
libraries/AP_Math/polygon.h
View File

@ -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);