diff options
Diffstat (limited to 'src/modules/navigator')
-rw-r--r-- | src/modules/navigator/mission_params.c | 15 | ||||
-rw-r--r-- | src/modules/navigator/module.mk | 2 | ||||
-rw-r--r-- | src/modules/navigator/navigator.h | 20 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 60 | ||||
-rw-r--r-- | src/modules/navigator/navigator_params.c | 96 | ||||
-rw-r--r-- | src/modules/navigator/rtl.cpp | 1 | ||||
-rw-r--r-- | src/modules/navigator/rtl.h | 1 | ||||
-rw-r--r-- | src/modules/navigator/rtl_params.c | 11 |
8 files changed, 39 insertions, 167 deletions
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 9696b3e53..f5067a70b 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -48,9 +48,20 @@ */ /** - * Loiter radius (fixed wing only) + * Take-off altitude * - * Default value of loiter radius (if not specified in mission item). + * Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to + * MIS_TAKEOFF_ALT on takeoff, then go to waypoint. + * + * @unit meters + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); + +/** + * Loiter radius after/during mission (FW only) + * + * Default value of loiter radius (fixedwing only). * * @unit meters * @min 0.0 diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index e2cc475da..88d8aac5d 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -38,10 +38,10 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ - navigator_params.c \ mission.cpp \ mission_params.c \ rtl.cpp \ + rtl_params.c \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index a6ab85519..9f877cd76 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -107,7 +107,6 @@ private: int _global_pos_sub; /**< global position subscription */ int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ @@ -143,25 +142,6 @@ private: bool _update_triplet; /**< flags if position setpoint triplet needs to be published */ - struct { - float acceptance_radius; - float loiter_radius; - int onboard_mission_enabled; - float takeoff_alt; - } _parameters; /**< local copies of parameters */ - - struct { - param_t acceptance_radius; - param_t loiter_radius; - param_t onboard_mission_enabled; - param_t takeoff_alt; - } _parameter_handles; /**< handles for parameters */ - - /** - * Update our local parameter cache. - */ - void parameters_update(); - /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e166058b9..3c8875a74 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -64,12 +64,10 @@ #include <uORB/uORB.h> #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_status.h> -#include <uORB/topics/parameter_update.h> #include <uORB/topics/mission.h> #include <uORB/topics/fence.h> #include <uORB/topics/navigation_capabilities.h> -#include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> #include <geo/geo.h> @@ -102,7 +100,6 @@ Navigator::Navigator() : _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), - _params_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), _pos_sp_triplet_pub(-1), @@ -127,14 +124,8 @@ Navigator::Navigator() : _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _update_triplet(false), - _parameters({}), - _parameter_handles({}) + _update_triplet(false) { - _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); - _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); - _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); - _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); } Navigator::~Navigator() @@ -163,23 +154,6 @@ Navigator::~Navigator() } void -Navigator::parameters_update() -{ - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius)); - param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); - param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); - param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - - //_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled); - - _geofence.updateParams(); -} - -void Navigator::global_position_update() { orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); @@ -251,7 +225,6 @@ Navigator::task_main() _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); @@ -259,7 +232,6 @@ Navigator::task_main() /* copy all topics first time */ vehicle_status_update(); vehicle_control_mode_update(); - parameters_update(); global_position_update(); home_position_update(); navigation_capabilities_update(); @@ -271,21 +243,19 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[6]; + struct pollfd fds[5]; /* Setup of loop */ - fds[0].fd = _params_sub; + fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; - fds[1].fd = _global_pos_sub; + fds[1].fd = _home_pos_sub; fds[1].events = POLLIN; - fds[2].fd = _home_pos_sub; + fds[2].fd = _capabilities_sub; fds[2].events = POLLIN; - fds[3].fd = _capabilities_sub; + fds[3].fd = _vstatus_sub; fds[3].events = POLLIN; - fds[4].fd = _vstatus_sub; + fds[4].fd = _control_mode_sub; fds[4].events = POLLIN; - fds[5].fd = _control_mode_sub; - fds[5].events = POLLIN; while (!_task_should_exit) { @@ -311,33 +281,27 @@ Navigator::task_main() } /* vehicle control mode updated */ - if (fds[5].revents & POLLIN) { + if (fds[4].revents & POLLIN) { vehicle_control_mode_update(); } /* vehicle status updated */ - if (fds[4].revents & POLLIN) { + if (fds[3].revents & POLLIN) { vehicle_status_update(); } - /* parameters updated */ - if (fds[0].revents & POLLIN) { - parameters_update(); - /* note that these new parameters won't be in effect until a mission triplet is published again */ - } - /* navigation capabilities updated */ - if (fds[3].revents & POLLIN) { + if (fds[2].revents & POLLIN) { navigation_capabilities_update(); } /* home position updated */ - if (fds[2].revents & POLLIN) { + if (fds[1].revents & POLLIN) { home_position_update(); } /* global position updated */ - if (fds[1].revents & POLLIN) { + if (fds[0].revents & POLLIN) { global_position_update(); /* Check geofence violation */ diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c deleted file mode 100644 index ad079a250..000000000 --- a/src/modules/navigator/navigator_params.c +++ /dev/null @@ -1,96 +0,0 @@ -/**************************************************************************** - * - * 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 navigator_params.c - * - * Parameters defined by the navigator task. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> - */ - -#include <nuttx/config.h> - -#include <systemlib/param/param.h> - -/* - * Navigator parameters, accessible via MAVLink - */ - -/** - * Waypoint acceptance radius - * - * Default value of acceptance radius (if not specified in mission item). - * - * @unit meters - * @min 0.0 - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); - -/** - * Loiter radius (fixed wing only) - * - * Default value of loiter radius (if not specified in mission item). - * - * @unit meters - * @min 0.0 - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); - -/** - * Enable onboard mission - * - * @group Navigation - */ -PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); - -/** - * Take-off altitude - * - * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); - -/** - * Enable parachute deployment - * - * @group Navigation - */ -PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 10499085b..d4e609584 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -58,6 +58,7 @@ RTL::RTL(Navigator *navigator, const char *name) : _home_position({}), _loiter_radius(50), _acceptance_radius(50), + _param_loiter_rad(this, "LOITER_RAD"), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), _param_land_delay(this, "LAND_DELAY") diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index a3a819403..d84fd1a6f 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -92,6 +92,7 @@ private: float _loiter_radius; float _acceptance_radius; + control::BlockParamFloat _param_loiter_rad; control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 7a8b1d745..bfe6ce7e1 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -48,6 +48,17 @@ */ /** + * Loiter radius after RTL (FW only) + * + * Default value of loiter radius after RTL (fixedwing only). + * + * @unit meters + * @min 0.0 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); + +/** * RTL altitude * * Altitude to fly back in RTL in meters |