From 328e84117ea8e9cfdec624f586e0b2f6ac06f8ae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 14 Jul 2017 13:17:30 +0200 Subject: [PATCH] navigator geofence: switch to new dataman data structure, support multiple polygons This also removes the 'navigator fence ' command to simplify code (I don't think there's still use for that anymore). However the file loading is still supported. If goefence.txt does not exist, navigator will not clear the geofence anymore on startup. --- src/modules/navigator/CMakeLists.txt | 3 + src/modules/navigator/geofence.cpp | 361 +++++++++++------- src/modules/navigator/geofence.h | 88 ++++- .../navigator/mission_feasibility_checker.cpp | 3 +- src/modules/navigator/navigator.h | 5 - src/modules/navigator/navigator_main.cpp | 40 +- 6 files changed, 315 insertions(+), 185 deletions(-) diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 55307ba4e2..6b17219c4e 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -30,6 +30,9 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + +include_directories(${PX4_SOURCE_DIR}/mavlink/include/mavlink) + px4_add_module( MODULE modules__navigator MAIN navigator diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 8918983f7f..4314030a79 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013,2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +36,7 @@ * * @author Jean Cyr * @author Thomas Gubler + * @author Beat Küng */ #include "geofence.h" #include "navigator.h" @@ -46,6 +47,9 @@ #include #include #include +#include + +#include "navigator.h" #define GEOFENCE_RANGE_WARNING_LIMIT 5000000 @@ -60,47 +64,135 @@ Geofence::Geofence(Navigator *navigator) : _param_max_ver_distance(this, "GF_MAX_VER_DIST", false) { updateParams(); + updateFence(); } -bool Geofence::inside(const struct vehicle_global_position_s &global_position) +Geofence::~Geofence() { - return inside(global_position.lat, global_position.lon, global_position.alt); + if (_polygons) { + delete[](_polygons); + } } -bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl) +void Geofence::updateFence() { - return inside(global_position.lat, global_position.lon, baro_altitude_amsl); + // initialize fence points count + mission_stats_entry_s stats; + int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + int num_fence_items = 0; + + if (ret == sizeof(mission_stats_entry_s)) { + num_fence_items = stats.num_items; + } + + // iterate over all polygons and store their starting vertices + _num_polygons = 0; + int current_seq = 1; + + while (current_seq <= num_fence_items) { + mission_fence_point_s mission_fence_point; + + if (dm_read(DM_KEY_FENCE_POINTS, current_seq, &mission_fence_point, sizeof(mission_fence_point_s)) != + sizeof(mission_fence_point_s)) { + PX4_ERR("dm_read failed"); + break; + } + + switch (mission_fence_point.nav_cmd) { + case MAV_CMD_NAV_FENCE_RETURN_POINT: + // TODO: do we need to store this? + ++current_seq; + break; + + case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION: + case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION: + if (mission_fence_point.vertex_count == 0) { + ++current_seq; // avoid endless loop + PX4_ERR("Polygon with 0 vertices. Skipping"); + + } else { + if (_polygons) { + // resize: this is somewhat inefficient, but we do not expect there to be many polygons + PolygonInfo *new_polygons = new PolygonInfo[_num_polygons + 1]; + + if (new_polygons) { + memcpy(new_polygons, _polygons, sizeof(PolygonInfo) * _num_polygons); + } + + delete[](_polygons); + _polygons = new_polygons; + + } else { + _polygons = new PolygonInfo[1]; + } + + if (!_polygons) { + _num_polygons = 0; + PX4_ERR("alloc failed"); + return; + } + + PolygonInfo &polygon = _polygons[_num_polygons]; + polygon.dataman_index = current_seq; + polygon.fence_type = mission_fence_point.nav_cmd; + polygon.vertex_count = mission_fence_point.vertex_count; + current_seq += mission_fence_point.vertex_count; + ++_num_polygons; + } + + break; + + default: + PX4_ERR("unhandled Fence command: %i", (int)mission_fence_point.nav_cmd); + ++current_seq; + break; + } + + } + } -bool Geofence::inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl) +bool Geofence::checkAll(const struct vehicle_global_position_s &global_position) +{ + return checkAll(global_position.lat, global_position.lon, global_position.alt); +} + +bool Geofence::checkAll(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl) +{ + return checkAll(global_position.lat, global_position.lon, baro_altitude_amsl); +} + + +bool Geofence::check(const struct vehicle_global_position_s &global_position, + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl, + const struct home_position_s home_pos, bool home_position_set) { if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { - return inside(global_position); + return checkAll(global_position); } else { - return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - (double)gps_position.alt * 1.0e-3); + return checkAll((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, + (double)gps_position.alt * 1.0e-3); } } else { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { - return inside(global_position, baro_altitude_amsl); + return checkAll(global_position, baro_altitude_amsl); } else { - return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - baro_altitude_amsl); + return checkAll((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, + baro_altitude_amsl); } } } -bool Geofence::inside(const struct mission_item_s &mission_item) +bool Geofence::check(const struct mission_item_s &mission_item) { - return inside(mission_item.lat, mission_item.lon, mission_item.altitude); + return checkAll(mission_item.lat, mission_item.lon, mission_item.altitude); } -bool Geofence::inside(double lat, double lon, float altitude) +bool Geofence::checkAll(double lat, double lon, float altitude) { bool inside_fence = true; @@ -141,7 +233,7 @@ bool Geofence::inside(double lat, double lon, float altitude) // to be inside the geofence both fences have to report being inside // as they both report being inside when not enabled - inside_fence = inside_fence && inside_polygon(lat, lon, altitude); + inside_fence = inside_fence && checkPolygons(lat, lon, altitude); if (inside_fence) { _outside_counter = 0; @@ -159,120 +251,92 @@ bool Geofence::inside(double lat, double lon, float altitude) } } -bool Geofence::inside_polygon(double lat, double lon, float altitude) + +bool Geofence::checkPolygons(double lat, double lon, float altitude) { - if (valid()) { - if (!isEmpty()) { - /* Vertical check */ - if (altitude > _altitude_max || altitude < _altitude_min) { - return false; - } - - /*Horizontal check */ - /* Adaptation of algorithm originally presented as - * PNPOLY - Point Inclusion in Polygon Test - * W. Randolph Franklin (WRF) */ - - bool c = false; - - struct fence_vertex_s temp_vertex_i; - struct fence_vertex_s temp_vertex_j; - - /* Red until fence is finished */ - for (unsigned i = 0, j = _vertices_count - 1; i < _vertices_count; j = i++) { - if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { - break; - } - - if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { - break; - } - - // skip vertex 0 (return point) - if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && - (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / - (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { - c = !c; - } - - } - - return c; - - } else { - /* Empty fence --> accept all points */ - return true; - } - - } else { - /* Invalid fence --> accept all points */ + if (isEmpty()) { + /* Empty fence -> accept all points */ return true; } + + /* Vertical check */ + if (altitude > _altitude_max || altitude < _altitude_min) { + return false; + } + + /* Horizontal check: iterate all polygons */ + bool outside_exclusion = true; + bool inside_inclusion = false; + bool had_inclusion_polygons = false; + + for (int polygon_idx = 0; polygon_idx < _num_polygons; ++polygon_idx) { + bool inside = insidePolygon(_polygons[polygon_idx], lat, lon, altitude); + + if (_polygons[polygon_idx].fence_type == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION) { + if (inside) { + inside_inclusion = true; + } + + had_inclusion_polygons = true; + + } else { // exclusion + if (inside) { + outside_exclusion = false; + } + } + } + + return (!had_inclusion_polygons || inside_inclusion) && outside_exclusion; +} + +bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude) +{ + + /* Adaptation of algorithm originally presented as + * PNPOLY - Point Inclusion in Polygon Test + * W. Randolph Franklin (WRF) + * Only supports non-complex polygons (not self intersecting) + */ + + mission_fence_point_s temp_vertex_i; + mission_fence_point_s temp_vertex_j; + bool c = false; + + for (unsigned i = 0, j = polygon.vertex_count - 1; i < polygon.vertex_count; j = i++) { + if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, &temp_vertex_i, + sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + break; + } + + if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + j, &temp_vertex_j, + sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + break; + } + + if (temp_vertex_i.frame != MAV_FRAME_GLOBAL && temp_vertex_i.frame != MAV_FRAME_GLOBAL_INT) { + // TODO: handle different frames + PX4_ERR("Frame type %i not supported", (int)temp_vertex_i.frame); + break; + } + + if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && + (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / + (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { + c = !c; + } + + // TODO: handle altitude + } + + return c; } bool Geofence::valid() { - // NULL fence is valid - if (isEmpty()) { - return true; - } - - // Otherwise - if ((_vertices_count < 4) || (_vertices_count > fence_s::GEOFENCE_MAX_VERTICES)) { - warnx("Fence must have at least 3 sides and not more than %d", fence_s::GEOFENCE_MAX_VERTICES - 1); - return false; - } - - return true; + return true; // always valid } -void -Geofence::addPoint(int argc, char *argv[]) -{ - int ix, last; - double lon, lat; - struct fence_vertex_s vertex; - char *end; - - if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) { - dm_clear(DM_KEY_FENCE_POINTS); - return; - } - - if (argc < 3) { - PX4_WARN("Specify: -clear | sequence latitude longitude [-publish]"); - } - - ix = atoi(argv[0]); - - if (ix >= DM_KEY_FENCE_POINTS_MAX) { - PX4_WARN("Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); - } - - lat = strtod(argv[1], &end); - lon = strtod(argv[2], &end); - - last = 0; - - if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) { - last = 1; - } - - vertex.lat = (float)lat; - vertex.lon = (float)lon; - - if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { - if (last) { - } - - return; - } - - PX4_WARN("can't store fence point"); -} - - int Geofence::loadFromFile(const char *filename) { @@ -317,36 +381,41 @@ Geofence::loadFromFile(const char *filename) if (gotVertical) { /* Parse the line as a geofence point */ - struct fence_vertex_s vertex; + mission_fence_point_s vertex; + vertex.frame = MAV_FRAME_GLOBAL; + vertex.nav_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION; + vertex.vertex_count = 0; // this will be filled in a second pass + vertex.alt = 0; // TODO: does this make sense? /* if the line starts with DMS, this means that the coordinate is given as degree minute second instead of decimal degrees */ if (line[textStart] == 'D' && line[textStart + 1] == 'M' && line[textStart + 2] == 'S') { /* Handle degree minute second format */ - float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s; + double lat_d, lat_m, lat_s, lon_d, lon_m, lon_s; - if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) { - warnx("Scanf to parse DMS geofence vertex failed."); + if (sscanf(line, "DMS %lf %lf %lf %lf %lf %lf", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) { + PX4_ERR("Scanf to parse DMS geofence vertex failed."); goto error; } -// warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); +// PX4_INFO("Geofence DMS: %.5lf %.5lf %.5lf ; %.5lf %.5lf %.5lf", lat_d, lat_m, lat_s, lon_d, lon_m, lon_s); - vertex.lat = lat_d + lat_m / 60.0f + lat_s / 3600.0f; - vertex.lon = lon_d + lon_m / 60.0f + lon_s / 3600.0f; + vertex.lat = lat_d + lat_m / 60.0 + lat_s / 3600.0; + vertex.lon = lon_d + lon_m / 60.0 + lon_s / 3600.0; } else { /* Handle decimal degree format */ - if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) { - warnx("Scanf to parse geofence vertex failed."); + if (sscanf(line, "%lf %lf", &vertex.lat, &vertex.lon) != 2) { + PX4_ERR("Scanf to parse geofence vertex failed."); goto error; } } - if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) { + if (dm_write(DM_KEY_FENCE_POINTS, pointCounter + 1, DM_PERSIST_POWER_ON_RESET, &vertex, + sizeof(vertex)) != sizeof(vertex)) { goto error; } - warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); + PX4_INFO("Geofence: point: %d, lat %.5lf: lon: %.5lf", pointCounter, vertex.lat, vertex.lon); pointCounter++; @@ -356,23 +425,40 @@ Geofence::loadFromFile(const char *filename) goto error; } - warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max); + PX4_INFO("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max); gotVertical = true; } } + /* Check if import was successful */ - if (gotVertical && pointCounter > 0) { - _vertices_count = pointCounter; - warnx("Geofence: imported successfully"); + if (gotVertical && pointCounter > 2) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported"); rc = PX4_OK; + /* do a second pass, now that we know the number of vertices */ + for (int seq = 1; seq <= pointCounter; ++seq) { + mission_fence_point_s mission_fence_point; + + if (dm_read(DM_KEY_FENCE_POINTS, seq, &mission_fence_point, sizeof(mission_fence_point_s)) == + sizeof(mission_fence_point_s)) { + mission_fence_point.vertex_count = pointCounter; + dm_write(DM_KEY_FENCE_POINTS, seq, DM_PERSIST_POWER_ON_RESET, &mission_fence_point, + sizeof(mission_fence_point_s)); + } + } + + mission_stats_entry_s stats; + stats.num_items = pointCounter; + rc = dm_write(DM_KEY_FENCE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s)); + } else { - warnx("Geofence: import error"); + PX4_ERR("Geofence: import error"); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence import error"); } + updateFence(); + error: fclose(fp); return rc; @@ -381,6 +467,7 @@ error: int Geofence::clearDm() { dm_clear(DM_KEY_FENCE_POINTS); + updateFence(); return PX4_OK; } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index f329282db6..8f601e9e58 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -67,7 +67,7 @@ public: Geofence(Navigator *navigator); Geofence(const Geofence &) = delete; Geofence &operator=(const Geofence &) = delete; - ~Geofence() = default; + ~Geofence(); /* Altitude mode, corresponding to the param GF_ALTMODE */ enum { @@ -82,31 +82,52 @@ public: }; /** - * Return whether system is inside geofence. - * - * Calculate whether point is inside arbitrary polygon - * @param craft pointer craft coordinates - * @return true: system is inside fence, false: system is outside fence + * update the geofence from dataman */ - bool inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl); + void updateFence(); - bool inside(const struct mission_item_s &mission_item); + /** + * Return whether the system obeys the geofence. + * + * @return true: system is obeying fence, false: system is violating fence + */ + bool check(const struct vehicle_global_position_s &global_position, + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl, + const struct home_position_s home_pos, bool home_position_set); - bool inside_polygon(double lat, double lon, float altitude); + /** + * Return whether a mission item obeys the geofence. + * + * @return true: system is obeying fence, false: system is violating fence + */ + bool check(const struct mission_item_s &mission_item); int clearDm(); bool valid(); /** - * Specify fence vertex position. + * Load a single inclusion polygon, replacing any already existing polygons. + * The file has one of the following formats: + * - Decimal Degrees: + * 0 900 + * 47.475273548913222 8.52672100067138672 + * 47.4608261578541359 8.53414535522460938 + * 47.4613484218861217 8.56444358825683594 + * 47.4830758091035534 8.53470325469970703 + * + * - Degree-Minute-Second: + * 0 900 + * DMS -26 -34 -10.4304 151 50 14.5428 + * DMS -26 -34 -11.8416 151 50 21.8580 + * DMS -26 -34 -36.5628 151 50 28.1112 + * DMS -26 -34 -37.1640 151 50 24.1620 + * + * Where the first line is min, max altitude in meters AMSL. */ - void addPoint(int argc, char *argv[]); - int loadFromFile(const char *filename); - bool isEmpty() {return _vertices_count == 0;} + bool isEmpty() { return _num_polygons == 0; } int getAltitudeMode() { return _param_altitude_mode.get(); } int getSource() { return _param_source.get(); } @@ -126,7 +147,13 @@ private: float _altitude_min{0.0f}; float _altitude_max{0.0f}; - unsigned _vertices_count{0}; + struct PolygonInfo { + uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_* + uint16_t dataman_index; + uint16_t vertex_count; + }; + PolygonInfo *_polygons{nullptr}; + int _num_polygons{0}; /* Params */ control::BlockParamInt _param_action; @@ -138,9 +165,34 @@ private: int _outside_counter{0}; - bool inside(double lat, double lon, float altitude); - bool inside(const struct vehicle_global_position_s &global_position); - bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); + /** + * Check if a point passes the Geofence test. + * This takes all polygons and minimum & maximum altitude into account + * + * The check passes if: (inside(polygon_inclusion_1) || inside(polygon_inclusion_2) || ... ) && + * !inside(polygon_exclusion_1) && !inside(polygon_exclusion_2) && ... + * && (altitude within [min, max]) + * or: no polygon configured + * @return result of the check above (false for a geofence violation) + */ + bool checkPolygons(double lat, double lon, float altitude); + + /** + * Check if a point passes the Geofence test. + * In addition to checkPolygons(), this takes all additional parameters into account. + * + * @return false for a geofence violation + */ + bool checkAll(double lat, double lon, float altitude); + + bool checkAll(const struct vehicle_global_position_s &global_position); + bool checkAll(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); + + /** + * Check if a single point is within a polygon + * @return true if within polygon + */ + bool insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude); }; #endif /* GEOFENCE_H_ */ diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index cdb4724a2a..29a379b32a 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -188,7 +188,8 @@ MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionIt // Geofence function checks against home altitude amsl missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude; - if (MissionBlock::item_contains_position(&missionitem) && !geofence.inside(missionitem)) { + if (MissionBlock::item_contains_position(&missionitem) && + !geofence.check(missionitem)) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %d", i + 1); return false; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 9ce8b9f9f9..849f1ee456 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -95,11 +95,6 @@ public: */ void status(); - /** - * Add point to geofence - */ - void add_fence_point(int argc, char *argv[]); - /** * Load fence from file */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e0a966a2c6..4e9efe705b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -226,18 +226,12 @@ Navigator::task_main() bool have_geofence_position_data = false; /* Try to load the geofence: - * if /fs/microsd/etc/geofence.txt load from this file - * else clear geofence data in datamanager */ + * if /fs/microsd/etc/geofence.txt load from this file */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { - PX4_INFO("Try to load geofence.txt"); + PX4_INFO("Loading geofence from %s", GEOFENCE_FILENAME); _geofence.loadFromFile(GEOFENCE_FILENAME); - - } else { - if (_geofence.clearDm() != OK) { - mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence"); - } } /* do subscriptions */ @@ -532,7 +526,8 @@ Navigator::task_main() (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); + bool inside = _geofence.check(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, + home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; @@ -701,12 +696,18 @@ Navigator::start() void Navigator::status() { - if (_geofence.valid()) { - PX4_INFO("Geofence is valid"); + /* TODO: add this again */ + // PX4_INFO("Global position is %svalid", _global_pos_valid ? "" : "in"); + + // if (_global_pos.global_valid) { + // PX4_INFO("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); + // PX4_INFO("Altitude %5.5f meters, altitude above home %5.5f meters", + // (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + // PX4_INFO("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", + // (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); + // PX4_INFO("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); + // } - } else { - PX4_INFO("Geofence not set (no /etc/geofence.txt on microsd) or not valid"); - } } void @@ -819,12 +820,6 @@ Navigator::get_acceptance_radius(float mission_item_radius) return radius; } -void -Navigator::add_fence_point(int argc, char *argv[]) -{ - _geofence.addPoint(argc, argv); -} - void Navigator::load_fence_from_file(const char *filename) { @@ -852,7 +847,7 @@ Navigator::abort_landing() static void usage() { - PX4_INFO("usage: navigator {start|stop|status|fence|fencefile}"); + PX4_INFO("usage: navigator {start|stop|status|fencefile}"); } int navigator_main(int argc, char *argv[]) @@ -898,9 +893,6 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "status")) { navigator::g_navigator->status(); - } else if (!strcmp(argv[1], "fence")) { - navigator::g_navigator->add_fence_point(argc - 2, argv + 2); - } else if (!strcmp(argv[1], "fencefile")) { navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME);