forked from Archive/PX4-Autopilot
geofence: do not keep fence in memory
This commit is contained in:
parent
099c2f5a00
commit
70d4ef480a
|
@ -57,9 +57,10 @@ static const int ERROR = -1;
|
||||||
|
|
||||||
Geofence::Geofence() : _fence_pub(-1),
|
Geofence::Geofence() : _fence_pub(-1),
|
||||||
_altitude_min(0),
|
_altitude_min(0),
|
||||||
_altitude_max(0)
|
_altitude_max(0),
|
||||||
|
_verticesCount(0)
|
||||||
{
|
{
|
||||||
memset(&_fence, 0, sizeof(_fence));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Geofence::~Geofence()
|
Geofence::~Geofence()
|
||||||
|
@ -79,6 +80,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
|
||||||
|
|
||||||
bool Geofence::inside(double lat, double lon, float altitude)
|
bool Geofence::inside(double lat, double lon, float altitude)
|
||||||
{
|
{
|
||||||
|
if (valid()) {
|
||||||
/* Vertical check */
|
/* Vertical check */
|
||||||
if (altitude > _altitude_max || altitude < _altitude_min)
|
if (altitude > _altitude_max || altitude < _altitude_min)
|
||||||
return false;
|
return false;
|
||||||
|
@ -88,50 +90,46 @@ bool Geofence::inside(double lat, double lon, float altitude)
|
||||||
* PNPOLY - Point Inclusion in Polygon Test
|
* PNPOLY - Point Inclusion in Polygon Test
|
||||||
* W. Randolph Franklin (WRF) */
|
* W. Randolph Franklin (WRF) */
|
||||||
|
|
||||||
unsigned int i, j, vertices = _fence.count;
|
|
||||||
bool c = false;
|
bool c = false;
|
||||||
|
|
||||||
// skip vertex 0 (return point)
|
struct fence_vertex_s temp_vertex_i;
|
||||||
for (i = 0, j = vertices - 1; i < vertices; j = i++)
|
struct fence_vertex_s temp_vertex_j;
|
||||||
if (((_fence.vertices[i].lon) >= lon != (_fence.vertices[j].lon >= lon)) &&
|
|
||||||
(lat <= (_fence.vertices[j].lat - _fence.vertices[i].lat) * (lon - _fence.vertices[i].lon) /
|
|
||||||
(_fence.vertices[j].lon - _fence.vertices[i].lon) + _fence.vertices[i].lat))
|
|
||||||
c = !c;
|
|
||||||
return c;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool
|
/* Red until fence is finished */
|
||||||
Geofence::loadFromDm(unsigned vertices)
|
for (int i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
|
||||||
{
|
if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
|
||||||
struct fence_s temp_fence;
|
|
||||||
|
|
||||||
unsigned i;
|
|
||||||
for (i = 0; i < vertices; i++) {
|
|
||||||
if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
temp_fence.count = i;
|
// skip vertex 0 (return point)
|
||||||
|
if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
|
||||||
|
(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
|
||||||
|
(temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
|
||||||
|
c = !c;
|
||||||
|
}
|
||||||
|
|
||||||
if (valid())
|
}
|
||||||
memcpy(&_fence, &temp_fence, sizeof(_fence));
|
|
||||||
else
|
|
||||||
warnx("Invalid fence file, ignored!");
|
|
||||||
|
|
||||||
return _fence.count != 0;
|
return c;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
Geofence::valid()
|
Geofence::valid()
|
||||||
{
|
{
|
||||||
// NULL fence is valid
|
// NULL fence is valid
|
||||||
if (_fence.count == 0) {
|
if (_verticesCount == 0) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Otherwise
|
// Otherwise
|
||||||
if ((_fence.count < 4) || (_fence.count > GEOFENCE_MAX_VERTICES)) {
|
if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) {
|
||||||
warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1);
|
warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -266,17 +264,11 @@ Geofence::loadFromFile(const char *filename)
|
||||||
|
|
||||||
fclose(fp);
|
fclose(fp);
|
||||||
|
|
||||||
/* Re-Load imported geofence from DM */
|
/* Check if import was successful */
|
||||||
if(gotVertical && pointCounter > 0)
|
if(gotVertical && pointCounter > 0)
|
||||||
{
|
{
|
||||||
bool fence_valid = loadFromDm(GEOFENCE_MAX_VERTICES);
|
_verticesCount = pointCounter;
|
||||||
if (fence_valid) {
|
warnx("Geofence: imported successfully");
|
||||||
warnx("Geofence: imported and loaded successfully");
|
|
||||||
return OK;
|
|
||||||
} else {
|
|
||||||
warnx("Geofence: datamanager read error");
|
|
||||||
return ERROR;
|
|
||||||
}
|
|
||||||
} else {
|
} else {
|
||||||
warnx("Geofence: import error");
|
warnx("Geofence: import error");
|
||||||
}
|
}
|
||||||
|
|
|
@ -46,11 +46,12 @@
|
||||||
|
|
||||||
class Geofence {
|
class Geofence {
|
||||||
private:
|
private:
|
||||||
struct fence_s _fence;
|
|
||||||
orb_advert_t _fence_pub; /**< publish fence topic */
|
orb_advert_t _fence_pub; /**< publish fence topic */
|
||||||
|
|
||||||
float _altitude_min;
|
float _altitude_min;
|
||||||
float _altitude_max;
|
float _altitude_max;
|
||||||
|
|
||||||
|
int _verticesCount;
|
||||||
public:
|
public:
|
||||||
Geofence();
|
Geofence();
|
||||||
~Geofence();
|
~Geofence();
|
||||||
|
@ -66,12 +67,6 @@ public:
|
||||||
bool inside(const struct vehicle_global_position_s *craft);
|
bool inside(const struct vehicle_global_position_s *craft);
|
||||||
bool inside(double lat, double lon, float altitude);
|
bool inside(double lat, double lon, float altitude);
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Load fence parameters.
|
|
||||||
*/
|
|
||||||
bool loadFromDm(unsigned vertices);
|
|
||||||
|
|
||||||
int clearDm();
|
int clearDm();
|
||||||
|
|
||||||
bool valid();
|
bool valid();
|
||||||
|
|
|
@ -547,8 +547,6 @@ Navigator::task_main()
|
||||||
|
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] started");
|
mavlink_log_info(_mavlink_fd, "[navigator] started");
|
||||||
|
|
||||||
_fence_valid = _geofence.loadFromDm(GEOFENCE_MAX_VERTICES);
|
|
||||||
|
|
||||||
/* Try to load the geofence:
|
/* Try to load the geofence:
|
||||||
* if /fs/microsd/etc/geofence.txt load from this file
|
* if /fs/microsd/etc/geofence.txt load from this file
|
||||||
* else clear geofence data in datamanager
|
* else clear geofence data in datamanager
|
||||||
|
|
Loading…
Reference in New Issue