From ec60a254d279e826c1d3d8097dcbe4bfa89d1e89 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 14:46:17 +0100 Subject: [PATCH 1/9] navigator: add pre mission geofence check --- src/modules/navigator/geofence.cpp | 9 +++++++-- src/modules/navigator/geofence.h | 1 + .../navigator/mission_feasibility_checker.cpp | 19 ++++++++++++++++++- 3 files changed, 26 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 199ccb41b0..666d9076e3 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -70,15 +70,20 @@ Geofence::~Geofence() bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { + double lat = vehicle->lat / 1e7d; + double lon = vehicle->lon / 1e7d; + return inside(lat, lon); +} + +bool Geofence::inside(double lat, double lon) +{ /* Adaptation of algorithm originally presented as * PNPOLY - Point Inclusion in Polygon Test * W. Randolph Franklin (WRF) */ unsigned int i, j, vertices = _fence.count; bool c = false; - double lat = vehicle->lat / 1e7d; - double lon = vehicle->lon / 1e7d; // skip vertex 0 (return point) for (i = 0, j = vertices - 1; i < vertices; j = i++) diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 8a1d06e71a..0335f58f61 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -64,6 +64,7 @@ public: * @return true: craft is inside fence, false:craft is outside fence */ bool inside(const struct vehicle_global_position_s *craft); + bool inside(double lat, double lon); /** diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index aba2dffff8..798e95faa5 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -97,7 +97,24 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { - //xxx: check geofence + /* Check if all mission items are inside the geofence (if we have a valid geofence) */ + if (geofence.valid()) { + for (size_t i = 0; i < nMissionItems; i++) { + static struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + if (!geofence.inside(missionitem.lat, missionitem.lon)) { + mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); + return false; + } + } + } + return true; } From 31d1f436adad9dd963797872fa3575319099e940 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 15:17:07 +0100 Subject: [PATCH 2/9] geofence: add simple vertical check --- src/modules/navigator/geofence.cpp | 10 ++++++++-- src/modules/navigator/geofence.h | 2 +- src/modules/navigator/mission_feasibility_checker.cpp | 2 +- 3 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 666d9076e3..f76597e607 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -72,12 +72,18 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { double lat = vehicle->lat / 1e7d; double lon = vehicle->lon / 1e7d; + float alt = vehicle->alt; - return inside(lat, lon); + return inside(lat, lon, vehicle->alt); } -bool Geofence::inside(double lat, double lon) +bool Geofence::inside(double lat, double lon, float altitude) { + /* 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) */ diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 0335f58f61..d834280ccf 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -64,7 +64,7 @@ public: * @return true: craft is inside fence, false:craft is outside fence */ bool inside(const struct vehicle_global_position_s *craft); - bool inside(double lat, double lon); + bool inside(double lat, double lon, float altitude); /** diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 798e95faa5..eaafa217de 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -108,7 +108,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (!geofence.inside(missionitem.lat, missionitem.lon)) { + if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); return false; } From 099c2f5a00f9789533888409f478f4157a5f88b6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 15:50:49 +0100 Subject: [PATCH 3/9] geofence: add DMS coordinate format support for textfile --- src/modules/navigator/geofence.cpp | 22 +++++++++++++++++++--- src/modules/navigator/navigator_main.cpp | 2 +- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index f76597e607..373429c084 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -225,12 +225,28 @@ Geofence::loadFromFile(const char *filename) /* Parse the line as a geofence point */ struct fence_vertex_s vertex; - if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) - return ERROR; + /* 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; + if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) + return 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); + + vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f; + vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f; + + } else { + /* Handle decimal degree format */ + + if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) + return ERROR; + } if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) - return ERROR; + return ERROR; warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e52ef16fa5..80e006d147 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1515,7 +1515,7 @@ void Navigator::load_fence_from_file(const char *filename) static void usage() { - errx(1, "usage: navigator {start|stop|status|fence}"); + errx(1, "usage: navigator {start|stop|status|fence|fencefile}"); } int navigator_main(int argc, char *argv[]) From 70d4ef480ac5461ef54ac72a54bd335007e233cc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 18:49:31 +0100 Subject: [PATCH 4/9] geofence: do not keep fence in memory --- src/modules/navigator/geofence.cpp | 86 +++++++++++------------- src/modules/navigator/geofence.h | 9 +-- src/modules/navigator/navigator_main.cpp | 2 - 3 files changed, 41 insertions(+), 56 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 373429c084..1063fd2e2c 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -57,9 +57,10 @@ static const int ERROR = -1; Geofence::Geofence() : _fence_pub(-1), _altitude_min(0), - _altitude_max(0) + _altitude_max(0), + _verticesCount(0) { - memset(&_fence, 0, sizeof(_fence)); + } Geofence::~Geofence() @@ -79,59 +80,56 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) bool Geofence::inside(double lat, double lon, float altitude) { - /* Vertical check */ - if (altitude > _altitude_max || altitude < _altitude_min) - return false; + if (valid()) { + /* 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) */ + /*Horizontal check */ + /* Adaptation of algorithm originally presented as + * PNPOLY - Point Inclusion in Polygon Test + * W. Randolph Franklin (WRF) */ - unsigned int i, j, vertices = _fence.count; - bool c = false; + bool c = false; - // skip vertex 0 (return point) - for (i = 0, j = vertices - 1; i < vertices; j = i++) - 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; -} + struct fence_vertex_s temp_vertex_i; + struct fence_vertex_s temp_vertex_j; -bool -Geofence::loadFromDm(unsigned vertices) -{ - struct fence_s temp_fence; + /* Red until fence is finished */ + 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)) { + 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 (((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; + } - 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; } + + return c; + + } else { + return true; } - - temp_fence.count = i; - - if (valid()) - memcpy(&_fence, &temp_fence, sizeof(_fence)); - else - warnx("Invalid fence file, ignored!"); - - return _fence.count != 0; } bool Geofence::valid() { // NULL fence is valid - if (_fence.count == 0) { + if (_verticesCount == 0) { return true; } // 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); return false; } @@ -266,17 +264,11 @@ Geofence::loadFromFile(const char *filename) fclose(fp); - /* Re-Load imported geofence from DM */ + /* Check if import was successful */ if(gotVertical && pointCounter > 0) { - bool fence_valid = loadFromDm(GEOFENCE_MAX_VERTICES); - if (fence_valid) { - warnx("Geofence: imported and loaded successfully"); - return OK; - } else { - warnx("Geofence: datamanager read error"); - return ERROR; - } + _verticesCount = pointCounter; + warnx("Geofence: imported successfully"); } else { warnx("Geofence: import error"); } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index d834280ccf..9c753a11de 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -46,11 +46,12 @@ class Geofence { private: - struct fence_s _fence; orb_advert_t _fence_pub; /**< publish fence topic */ float _altitude_min; float _altitude_max; + + int _verticesCount; public: Geofence(); ~Geofence(); @@ -66,12 +67,6 @@ public: bool inside(const struct vehicle_global_position_s *craft); bool inside(double lat, double lon, float altitude); - - /** - * Load fence parameters. - */ - bool loadFromDm(unsigned vertices); - int clearDm(); bool valid(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 80e006d147..8b5cc45767 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -547,8 +547,6 @@ Navigator::task_main() mavlink_log_info(_mavlink_fd, "[navigator] started"); - _fence_valid = _geofence.loadFromDm(GEOFENCE_MAX_VERTICES); - /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager From 819822e1722dc31c0f97b8494c96a6c292b07185 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 22:01:31 +0100 Subject: [PATCH 5/9] navigator/geofence: add isEmpty() function and checks --- src/modules/navigator/geofence.cpp | 62 ++++++++++++++++-------------- src/modules/navigator/geofence.h | 4 +- 2 files changed, 37 insertions(+), 29 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 1063fd2e2c..a49a71e1b8 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -81,41 +81,48 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) bool Geofence::inside(double lat, double lon, float altitude) { if (valid()) { - /* 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) */ + if (!isEmpty()) { + /* Vertical check */ + if (altitude > _altitude_max || altitude < _altitude_min) + return false; - bool c = false; + /*Horizontal check */ + /* Adaptation of algorithm originally presented as + * PNPOLY - Point Inclusion in Polygon Test + * W. Randolph Franklin (WRF) */ - struct fence_vertex_s temp_vertex_i; - struct fence_vertex_s temp_vertex_j; + 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 = _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)) { + 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 (((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; + } - /* Red until fence is finished */ - 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)) { - 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 (((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; } + return c; + } else { + /* Empty fence --> accept all points */ + return true; } - return c; - } else { + /* Invalid fence --> accept all points */ return true; } } @@ -124,9 +131,8 @@ bool Geofence::valid() { // NULL fence is valid - if (_verticesCount == 0) { + if (isEmpty()) return true; - } // Otherwise if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) { diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 9c753a11de..781e7a263e 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -51,7 +51,7 @@ private: float _altitude_min; float _altitude_max; - int _verticesCount; + unsigned _verticesCount; public: Geofence(); ~Geofence(); @@ -79,6 +79,8 @@ public: void publishFence(unsigned vertices); int loadFromFile(const char *filename); + + bool isEmpty() {return _verticesCount == 0;} }; From 26af21619b3f2a67c2480872e0f7c14d0572626e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Jan 2014 10:49:16 +0100 Subject: [PATCH 6/9] navigator/geofence: add parameter to disable geofence --- src/modules/navigator/geofence.cpp | 15 ++++++- src/modules/navigator/geofence.h | 6 +++ src/modules/navigator/geofence_params.c | 55 ++++++++++++++++++++++++ src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator_main.cpp | 2 + 5 files changed, 78 insertions(+), 3 deletions(-) create mode 100644 src/modules/navigator/geofence_params.c diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index a49a71e1b8..9bbaf167a3 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -58,9 +58,11 @@ static const int ERROR = -1; Geofence::Geofence() : _fence_pub(-1), _altitude_min(0), _altitude_max(0), - _verticesCount(0) + _verticesCount(0), + param_geofence_on(NULL, "GF_ON", false) { - + /* Load initial params */ + updateParams(); } Geofence::~Geofence() @@ -80,6 +82,10 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) bool Geofence::inside(double lat, double lon, float altitude) { + /* Return true if geofence is disabled */ + if (param_geofence_on.get() != 1) + return true; + if (valid()) { if (!isEmpty()) { @@ -286,3 +292,8 @@ int Geofence::clearDm() { dm_clear(DM_KEY_FENCE_POINTS); } + +void Geofence::updateParams() +{ + param_geofence_on.update(); +} diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 781e7a263e..5b56ebc7a5 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -41,6 +41,7 @@ #define GEOFENCE_H_ #include +#include #define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt" @@ -52,6 +53,9 @@ private: float _altitude_max; unsigned _verticesCount; + + /* Params */ + control::BlockParamInt param_geofence_on; public: Geofence(); ~Geofence(); @@ -81,6 +85,8 @@ public: int loadFromFile(const char *filename); bool isEmpty() {return _verticesCount == 0;} + + void updateParams(); }; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c new file mode 100644 index 0000000000..20dd1fe2f0 --- /dev/null +++ b/src/modules/navigator/geofence_params.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file geofence_params.c + * + * Parameters for geofence + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * geofence parameters, accessible via MAVLink + * + */ + +// @DisplayName Switch to enable geofence +// @Description if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present +// @Range 0 or 1 +PARAM_DEFINE_INT32(GF_ON, 1); diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index b7900e84ed..55f8a4caac 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -41,6 +41,7 @@ SRCS = navigator_main.cpp \ navigator_params.c \ navigator_mission.cpp \ mission_feasibility_checker.cpp \ - geofence.cpp + geofence.cpp \ + geofence_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8b5cc45767..b6059a01ef 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -458,6 +458,8 @@ Navigator::parameters_update() param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); + + _geofence.updateParams(); } void From 95c20ba9f9cc57fffb64f13c7108f4cbb149bf7b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Jan 2014 11:29:06 +0100 Subject: [PATCH 7/9] add inflight geofence check, issues warning on gcs for now --- src/modules/navigator/navigator_main.cpp | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b6059a01ef..bb7520a03f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -160,6 +160,8 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ Geofence _geofence; + bool _geofence_violation_warning_sent; + bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ @@ -393,7 +395,8 @@ Navigator::Navigator() : _time_first_inside_orbit(0), _set_nav_state_timestamp(0), _need_takeoff(true), - _do_takeoff(false) + _do_takeoff(false), + _geofence_violation_warning_sent(false) { _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); @@ -766,6 +769,21 @@ Navigator::task_main() on_mission_item_reached(); } } + + /* Check geofence violation */ + if(!_geofence.inside(&_global_pos)) { + //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) + + /* Issue a warning about the geofence violation once */ + if (!_geofence_violation_warning_sent) + { + mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); + _geofence_violation_warning_sent = true; + } + } else { + /* Reset the _geofence_violation_warning_sent field */ + _geofence_violation_warning_sent = false; + } } /* notify user about state changes */ From d1e991f1f0183ce6855bf2df15e1fdd311d096d4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Jan 2014 12:20:25 +0100 Subject: [PATCH 8/9] launchdetection: add minimal throttle, fix parameter update --- src/lib/launchdetection/LaunchDetector.cpp | 8 +++++--- src/lib/launchdetection/LaunchDetector.h | 3 +++ src/lib/launchdetection/launchdetection_params.c | 11 ++++++++--- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 4 files changed, 17 insertions(+), 7 deletions(-) diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 2545e0a7e2..d894d6a6fb 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -43,7 +43,8 @@ #include LaunchDetector::LaunchDetector() : - launchdetection_on(NULL, "LAUN_ALL_ON", false) + launchdetection_on(NULL, "LAUN_ALL_ON", false), + throttle_min(NULL, "LAUN_THR_MIN", false) { /* init all detectors */ launchMethods[0] = new CatapultLaunchMethod(); @@ -83,10 +84,11 @@ bool LaunchDetector::getLaunchDetected() void LaunchDetector::updateParams() { warnx(" LaunchDetector::updateParams()"); - //launchdetection_on.update(); + launchdetection_on.update(); + throttle_min.update(); for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { - //launchMethods[i]->updateParams(); + launchMethods[i]->updateParams(); warnx("updating component %d", i); } diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 9818911437..3b8aa0ced0 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -59,11 +59,14 @@ public: void updateParams(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; + float getMinThrottle() {return throttle_min.get(); } + // virtual bool getLaunchDetected(); protected: private: LaunchMethod* launchMethods[1]; control::BlockParamInt launchdetection_on; + control::BlockParamFloat throttle_min; }; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 536749c88c..e07a2b26da 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -33,11 +33,11 @@ ****************************************************************************/ /** - * @file fw_pos_control_l1_params.c + * @file launchdetection_params.c * - * Parameters defined by the L1 position control task + * Parameters for launchdetection * - * @author Lorenz Meier + * @author Thomas Gubler */ #include @@ -65,3 +65,8 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); // @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection // @Range > 0, in seconds PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); + +// @DisplayName Throttle setting while detecting the launch +// @Description The throttle is set to this value while the system is waiting for the takeoff +// @Range 0 to 1 +PARAM_DEFINE_FLOAT(LAUN_THR_MIN, 0.0f); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3405185e21..826ee0cdb9 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1028,7 +1028,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } else { - throttle_max = 0.0f; + throttle_max = launchDetector.getMinThrottle(); } } From f387c0ccc3fca38a2989b784847743b752a78de6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Jan 2014 14:19:19 +0100 Subject: [PATCH 9/9] launchdetection: rename pre takeoff throttle param and fix usage --- src/lib/launchdetection/LaunchDetector.cpp | 4 ++-- src/lib/launchdetection/LaunchDetector.h | 4 ++-- src/lib/launchdetection/launchdetection_params.c | 2 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 ++++++++++++-- 4 files changed, 17 insertions(+), 7 deletions(-) diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index d894d6a6fb..67b32ad82c 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -44,7 +44,7 @@ LaunchDetector::LaunchDetector() : launchdetection_on(NULL, "LAUN_ALL_ON", false), - throttle_min(NULL, "LAUN_THR_MIN", false) + throttlePreTakeoff(NULL, "LAUN_THR_PRE", false) { /* init all detectors */ launchMethods[0] = new CatapultLaunchMethod(); @@ -85,7 +85,7 @@ void LaunchDetector::updateParams() { warnx(" LaunchDetector::updateParams()"); launchdetection_on.update(); - throttle_min.update(); + throttlePreTakeoff.update(); for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { launchMethods[i]->updateParams(); diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 3b8aa0ced0..7c2ff826cf 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -59,14 +59,14 @@ public: void updateParams(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; - float getMinThrottle() {return throttle_min.get(); } + float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } // virtual bool getLaunchDetected(); protected: private: LaunchMethod* launchMethods[1]; control::BlockParamInt launchdetection_on; - control::BlockParamFloat throttle_min; + control::BlockParamFloat throttlePreTakeoff; }; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index e07a2b26da..63a8981aa8 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -69,4 +69,4 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); // @DisplayName Throttle setting while detecting the launch // @Description The throttle is set to this value while the system is waiting for the takeoff // @Range 0 to 1 -PARAM_DEFINE_FLOAT(LAUN_THR_MIN, 0.0f); +PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 826ee0cdb9..04caf0bbc3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -174,6 +174,7 @@ private: /* takeoff/launch states */ bool launch_detected; + bool usePreTakeoffThrust; bool launch_detection_message_sent; /* Landingslope object */ @@ -396,6 +397,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), launchDetector(), launch_detected(false), + usePreTakeoffThrust(false), launch_detection_message_sent(false) { /* safely initialize structs */ @@ -1004,6 +1006,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _att_sp.yaw_body = _l1_control.nav_bearing(); if (launch_detected) { + usePreTakeoffThrust = false; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (altitude_error > 15.0f) { @@ -1028,7 +1031,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } else { - throttle_max = launchDetector.getMinThrottle(); + usePreTakeoffThrust = true; } } @@ -1053,6 +1056,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* reset takeoff/launch state */ if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { launch_detected = false; + usePreTakeoffThrust = false; launch_detection_message_sent = false; } @@ -1150,8 +1154,14 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio setpoint = false; } + if (usePreTakeoffThrust) { + _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } + else { + _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); + } _att_sp.pitch_body = _tecs.get_pitch_demand(); - _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); + return setpoint; }