diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 2545e0a7e2..67b32ad82c 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), + throttlePreTakeoff(NULL, "LAUN_THR_PRE", 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(); + throttlePreTakeoff.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..7c2ff826cf 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 getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } + // virtual bool getLaunchDetected(); protected: private: LaunchMethod* launchMethods[1]; control::BlockParamInt launchdetection_on; + control::BlockParamFloat throttlePreTakeoff; }; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 536749c88c..63a8981aa8 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_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 3405185e21..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 = 0.0f; + 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; } diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 199ccb41b0..9bbaf167a3 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -57,9 +57,12 @@ static const int ERROR = -1; Geofence::Geofence() : _fence_pub(-1), _altitude_min(0), - _altitude_max(0) + _altitude_max(0), + _verticesCount(0), + param_geofence_on(NULL, "GF_ON", false) { - memset(&_fence, 0, sizeof(_fence)); + /* Load initial params */ + updateParams(); } Geofence::~Geofence() @@ -70,57 +73,75 @@ Geofence::~Geofence() bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { - - /* 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; + float alt = vehicle->alt; - // 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; + return inside(lat, lon, vehicle->alt); } -bool -Geofence::loadFromDm(unsigned vertices) +bool Geofence::inside(double lat, double lon, float altitude) { - struct fence_s temp_fence; + /* Return true if geofence is disabled */ + if (param_geofence_on.get() != 1) + return true; - 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; + 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 = _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; } + + } else { + /* Invalid fence --> accept all points */ + 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 (isEmpty()) 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; } @@ -214,12 +235,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); @@ -239,17 +276,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"); } @@ -261,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 8a1d06e71a..5b56ebc7a5 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -41,16 +41,21 @@ #define GEOFENCE_H_ #include +#include #define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt" class Geofence { private: - struct fence_s _fence; orb_advert_t _fence_pub; /**< publish fence topic */ float _altitude_min; float _altitude_max; + + unsigned _verticesCount; + + /* Params */ + control::BlockParamInt param_geofence_on; public: Geofence(); ~Geofence(); @@ -64,12 +69,7 @@ public: * @return true: craft is inside fence, false:craft is outside fence */ bool inside(const struct vehicle_global_position_s *craft); - - - /** - * Load fence parameters. - */ - bool loadFromDm(unsigned vertices); + bool inside(double lat, double lon, float altitude); int clearDm(); @@ -83,6 +83,10 @@ public: void publishFence(unsigned vertices); 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/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index aba2dffff8..eaafa217de 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, missionitem.altitude)) { //xxx: handle relative altitude + mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); + return false; + } + } + } + return true; } 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 e52ef16fa5..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"); @@ -458,6 +461,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 @@ -547,8 +552,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 @@ -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 */ @@ -1515,7 +1533,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[])