From c745c8bb451a4fd4f873cd49f30ae260b075b584 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Tue, 12 Jan 2021 10:49:02 +0100 Subject: [PATCH] Small navigator refactor/cleanup --- .../GeofenceBreachAvoidance/fake_geofence.hpp | 42 ++- .../geofence_breach_avoidance.cpp | 5 - .../geofence_breach_avoidance.h | 4 +- src/modules/navigator/navigator.h | 3 + src/modules/navigator/navigator_main.cpp | 258 +++++++++--------- 5 files changed, 176 insertions(+), 136 deletions(-) diff --git a/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp b/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp index d49a883227..d6c8faa044 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp @@ -1,4 +1,42 @@ - +/**************************************************************************** + * + * Copyright (c) 2013 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 + * 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 fake_geofence.h + * Provides a fake geofence interface to use for testing + * + * @author Roman Bapst + * @author Julian Kent + */ #include"../geofence.h" @@ -180,4 +218,4 @@ extern "C" { dm_restart( dm_reset_reason restart_type /* The last reset type */ ) {return 0;}; -} \ No newline at end of file +} diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp index aab784811a..1185e73f7d 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp @@ -51,11 +51,6 @@ GeofenceBreachAvoidance::GeofenceBreachAvoidance() : updateParameters(); } -GeofenceBreachAvoidance::~GeofenceBreachAvoidance() -{ - -} - void GeofenceBreachAvoidance::updateParameters() { param_get(_paramHandle.param_mpc_jerk_max, &_params.param_mpc_jerk_max); diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h index 5d5503f236..8fdbc6dc18 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h @@ -53,7 +53,7 @@ class GeofenceBreachAvoidance : public ModuleParams public: GeofenceBreachAvoidance(); - ~GeofenceBreachAvoidance(); + ~GeofenceBreachAvoidance() = default; matrix::Vector2 getFenceViolationTestPoint(); @@ -143,4 +143,4 @@ private: matrix::Vector2 waypointFromHomeToTestPointAtDist(float distance); -}; \ No newline at end of file +}; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index f1493ecba5..4aa4f16da0 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -297,6 +297,8 @@ public: bool abort_landing(); + void geofence_breach_check(bool &have_geofence_position_data); + // Param access float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); } float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); } @@ -381,6 +383,7 @@ private: Geofence _geofence; /**< class that handles the geofence */ bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */ GeofenceBreachAvoidance _gf_breach_avoidance; + hrt_abstime _last_geofence_check = 0; bool _can_loiter_at_sp{false}; /**< flags if current position SP can be used to loiter */ bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d02fd4a873..2613589d3d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -157,8 +157,6 @@ Navigator::run() /* rate-limit position subscription to 20 Hz / 50 ms */ orb_set_interval(_local_pos_sub, 50); - hrt_abstime last_geofence_check = 0; - while (!should_exit()) { /* wait for up to 1000ms for data */ @@ -480,131 +478,7 @@ Navigator::run() check_traffic(); /* Check geofence violation */ - if (have_geofence_position_data && - (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && - (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { - - const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - - matrix::Vector2 fence_violation_test_point; - geofence_violation_type_u gf_violation_type{}; - float test_point_bearing; - float test_point_distance; - float vertical_test_point_distance; - - if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx); - const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy); - _gf_breach_avoidance.setHorizontalVelocity(velocity_hor_abs); - _gf_breach_avoidance.setClimbRate(-_local_pos.vz); - test_point_distance = _gf_breach_avoidance.computeBrakingDistanceMultirotor(); - vertical_test_point_distance = _gf_breach_avoidance.computeVerticalBrakingDistanceMultirotor(); - - } else { - test_point_distance = 2.0f * get_loiter_radius(); - vertical_test_point_distance = 5.0f; - - if (hrt_absolute_time() - pos_ctrl_status.timestamp < 100000 && PX4_ISFINITE(pos_ctrl_status.nav_bearing)) { - test_point_bearing = pos_ctrl_status.nav_bearing; - - } else { - test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx); - } - } - - _gf_breach_avoidance.setHorizontalTestPointDistance(test_point_distance); - _gf_breach_avoidance.setVerticalTestPointDistance(vertical_test_point_distance); - _gf_breach_avoidance.setTestPointBearing(test_point_bearing); - _gf_breach_avoidance.setCurrentPosition(_global_pos.lat, _global_pos.lon, _global_pos.alt); - _gf_breach_avoidance.setMaxHorDistHome(_geofence.getMaxHorDistanceHome()); - _gf_breach_avoidance.setMaxVerDistHome(_geofence.getMaxVerDistanceHome()); - - if (home_position_valid()) { - _gf_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt); - } - - fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint(); - - gf_violation_type.flags.dist_to_home_exceeded = !_geofence.isCloserThanMaxDistToHome(fence_violation_test_point(0), - fence_violation_test_point(1), - _global_pos.alt); - - gf_violation_type.flags.max_altitude_exceeded = !_geofence.isBelowMaxAltitude(_global_pos.alt + - vertical_test_point_distance); - - gf_violation_type.flags.fence_violation = !_geofence.isInsidePolygonOrCircle(fence_violation_test_point(0), - fence_violation_test_point(1), - _global_pos.alt); - - last_geofence_check = hrt_absolute_time(); - have_geofence_position_data = false; - - _geofence_result.timestamp = hrt_absolute_time(); - _geofence_result.geofence_action = _geofence.getGeofenceAction(); - _geofence_result.home_required = _geofence.isHomeRequired(); - - if (gf_violation_type.value) { - /* inform other apps via the mission result */ - _geofence_result.geofence_violated = true; - - /* Issue a warning about the geofence violation once */ - if (!_geofence_violation_warning_sent) { - mavlink_log_critical(&_mavlink_log_pub, "Approaching on Geofence"); - - // we have predicted a geofence violation and if the action is to loiter then - // demand a reposition to a location which is inside the geofence - if (_geofence.getGeofenceAction() == geofence_result_s::GF_ACTION_LOITER) { - position_setpoint_triplet_s *rep = get_reposition_triplet(); - - matrix::Vector2 lointer_center_lat_lon; - matrix::Vector2 current_pos_lat_lon(_global_pos.lat, _global_pos.lon); - float loiter_altitude_amsl = _global_pos.alt; - - - if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - // the computation of the braking distance does not match the actual braking distance. Until we have a better model - // we set the loiter point to the current position, that will make sure that the vehicle will loiter inside the fence - lointer_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForMultirotor(gf_violation_type, - &_geofence); - - loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForMulticopter(gf_violation_type); - - } else { - - lointer_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForFixedWing(gf_violation_type, &_geofence); - loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForFixedWing(gf_violation_type); - } - - rep->current.timestamp = hrt_absolute_time(); - rep->current.yaw = get_local_position()->heading; - rep->current.yaw_valid = true; - rep->current.lat = lointer_center_lat_lon(0); - rep->current.lon = lointer_center_lat_lon(1); - rep->current.alt = loiter_altitude_amsl; - rep->current.valid = true; - rep->current.loiter_radius = get_loiter_radius(); - rep->current.alt_valid = true; - rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; - rep->current.loiter_direction = 1; - rep->current.cruising_throttle = get_cruising_throttle(); - rep->current.acceptance_radius = get_acceptance_radius(); - rep->current.cruising_speed = get_cruising_speed(); - - } - - _geofence_violation_warning_sent = true; - } - - } else { - /* inform other apps via the mission result */ - _geofence_result.geofence_violated = false; - - /* Reset the _geofence_violation_warning_sent field */ - _geofence_violation_warning_sent = false; - } - - _geofence_result_pub.publish(_geofence_result); - } + geofence_breach_check(have_geofence_position_data); /* Do stuff according to navigation state set by commander */ NavigatorMode *navigation_mode_new{nullptr}; @@ -822,6 +696,136 @@ Navigator::run() } } +void Navigator::geofence_breach_check(bool &have_geofence_position_data) +{ + + if (have_geofence_position_data && + (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && + (hrt_elapsed_time(&_last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { + + const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); + + matrix::Vector2 fence_violation_test_point; + geofence_violation_type_u gf_violation_type{}; + float test_point_bearing; + float test_point_distance; + float vertical_test_point_distance; + + if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx); + const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy); + _gf_breach_avoidance.setHorizontalVelocity(velocity_hor_abs); + _gf_breach_avoidance.setClimbRate(-_local_pos.vz); + test_point_distance = _gf_breach_avoidance.computeBrakingDistanceMultirotor(); + vertical_test_point_distance = _gf_breach_avoidance.computeVerticalBrakingDistanceMultirotor(); + + } else { + test_point_distance = 2.0f * get_loiter_radius(); + vertical_test_point_distance = 5.0f; + + if (hrt_absolute_time() - pos_ctrl_status.timestamp < 100000 && PX4_ISFINITE(pos_ctrl_status.nav_bearing)) { + test_point_bearing = pos_ctrl_status.nav_bearing; + + } else { + test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx); + } + } + + _gf_breach_avoidance.setHorizontalTestPointDistance(test_point_distance); + _gf_breach_avoidance.setVerticalTestPointDistance(vertical_test_point_distance); + _gf_breach_avoidance.setTestPointBearing(test_point_bearing); + _gf_breach_avoidance.setCurrentPosition(_global_pos.lat, _global_pos.lon, _global_pos.alt); + _gf_breach_avoidance.setMaxHorDistHome(_geofence.getMaxHorDistanceHome()); + _gf_breach_avoidance.setMaxVerDistHome(_geofence.getMaxVerDistanceHome()); + + if (home_position_valid()) { + _gf_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt); + } + + fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint(); + + gf_violation_type.flags.dist_to_home_exceeded = !_geofence.isCloserThanMaxDistToHome(fence_violation_test_point(0), + fence_violation_test_point(1), + _global_pos.alt); + + gf_violation_type.flags.max_altitude_exceeded = !_geofence.isBelowMaxAltitude(_global_pos.alt + + vertical_test_point_distance); + + gf_violation_type.flags.fence_violation = !_geofence.isInsidePolygonOrCircle(fence_violation_test_point(0), + fence_violation_test_point(1), + _global_pos.alt); + + _last_geofence_check = hrt_absolute_time(); + have_geofence_position_data = false; + + _geofence_result.timestamp = hrt_absolute_time(); + _geofence_result.geofence_action = _geofence.getGeofenceAction(); + _geofence_result.home_required = _geofence.isHomeRequired(); + + if (gf_violation_type.value) { + /* inform other apps via the mission result */ + _geofence_result.geofence_violated = true; + + /* Issue a warning about the geofence violation once */ + if (!_geofence_violation_warning_sent) { + mavlink_log_critical(&_mavlink_log_pub, "Approaching on Geofence"); + + // we have predicted a geofence violation and if the action is to loiter then + // demand a reposition to a location which is inside the geofence + if (_geofence.getGeofenceAction() == geofence_result_s::GF_ACTION_LOITER) { + position_setpoint_triplet_s *rep = get_reposition_triplet(); + + matrix::Vector2 lointer_center_lat_lon; + matrix::Vector2 current_pos_lat_lon(_global_pos.lat, _global_pos.lon); + float loiter_altitude_amsl = _global_pos.alt; + + + if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + // the computation of the braking distance does not match the actual braking distance. Until we have a better model + // we set the loiter point to the current position, that will make sure that the vehicle will loiter inside the fence + lointer_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForMultirotor(gf_violation_type, + &_geofence); + + loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForMulticopter(gf_violation_type); + + } else { + + lointer_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForFixedWing(gf_violation_type, &_geofence); + loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForFixedWing(gf_violation_type); + } + + rep->current.timestamp = hrt_absolute_time(); + rep->current.yaw = get_local_position()->heading; + rep->current.yaw_valid = true; + rep->current.lat = lointer_center_lat_lon(0); + rep->current.lon = lointer_center_lat_lon(1); + rep->current.alt = loiter_altitude_amsl; + rep->current.valid = true; + rep->current.loiter_radius = get_loiter_radius(); + rep->current.alt_valid = true; + rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + rep->current.loiter_direction = 1; + rep->current.cruising_throttle = get_cruising_throttle(); + rep->current.acceptance_radius = get_acceptance_radius(); + rep->current.cruising_speed = get_cruising_speed(); + + } + + _geofence_violation_warning_sent = true; + } + + } else { + /* inform other apps via the mission result */ + _geofence_result.geofence_violated = false; + + /* Reset the _geofence_violation_warning_sent field */ + _geofence_violation_warning_sent = false; + } + + _geofence_result_pub.publish(_geofence_result); + } +} + int Navigator::task_spawn(int argc, char *argv[]) { _task_id = px4_task_spawn_cmd("navigator",