From 85d7fdc741a39184d251e2d35d914a6506d6ecd1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 18:13:45 +0200 Subject: navigator: param enhancements --- src/modules/navigator/mission.cpp | 3 +- src/modules/navigator/mission.h | 1 - src/modules/navigator/mission_params.c | 10 ------ src/modules/navigator/module.mk | 1 + src/modules/navigator/navigator.h | 15 +++++++-- src/modules/navigator/navigator_main.cpp | 26 ++++++++++++--- src/modules/navigator/navigator_params.c | 55 ++++++++++++++++++++++++++++++++ 7 files changed, 93 insertions(+), 18 deletions(-) create mode 100644 src/modules/navigator/navigator_params.c (limited to 'src') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 167f24ca6..93007d939 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -60,7 +60,6 @@ Mission::Mission(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), MissionBlock(navigator), _param_onboard_enabled(this, "ONBOARD_EN"), - _param_loiter_radius(this, "LOITER_RAD"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), @@ -241,6 +240,8 @@ Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos bool Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { + /* make sure param is up to date */ + updateParams(); if (_param_onboard_enabled.get() > 0 && _current_onboard_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index ad8cb467c..cb3242c87 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -157,7 +157,6 @@ private: void publish_mission_result(); control::BlockParamFloat _param_onboard_enabled; - control::BlockParamFloat _param_loiter_radius; struct mission_s _onboard_mission; struct mission_s _offboard_mission; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index f5067a70b..8692328db 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -58,16 +58,6 @@ */ 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 - * @group Mission - */ -PARAM_DEFINE_FLOAT(MIS_LOITER_RAD, 50.0f); /** * Enable onboard mission diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index c7cba64cc..a1e109030 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ + navigator_params.c \ navigator_mode.cpp \ mission_block.cpp \ mission.cpp \ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 476e9a36c..b22da7117 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -42,11 +42,15 @@ #include +#include +#include + #include #include #include #include #include +#include #include "navigator_mode.h" #include "mission.h" @@ -54,7 +58,7 @@ #include "rtl.h" #include "geofence.h" -class Navigator +class Navigator : public control::SuperBlock { public: /** @@ -105,7 +109,7 @@ public: Geofence& get_geofence() { return _geofence; } bool get_is_in_loiter() { return _is_in_loiter; } - float get_loiter_radius() { return 50.0f; }; /* TODO: make param*/ + float get_loiter_radius() { return _param_loiter_radius.get(); }; private: @@ -121,6 +125,7 @@ private: int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ + int _param_update_sub; /**< param update subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ @@ -150,6 +155,7 @@ private: bool _is_in_loiter; /**< flags if current position SP can be used to loiter */ bool _update_triplet; /**< flags if position SP triplet needs to be published */ + control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ /** * Retrieve global position */ @@ -175,6 +181,11 @@ private: */ void vehicle_control_mode_update(); + /** + * Update parameters + */ + void params_update(); + /** * Shim for calling task_main from task_create. */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fe1f4893e..b1658a6b8 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -92,8 +92,7 @@ Navigator *g_navigator; } Navigator::Navigator() : - - /* state machine transition table */ + SuperBlock(NULL, "NAV"), _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), @@ -122,8 +121,10 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), - _update_triplet(false) + _update_triplet(false), + _param_loiter_radius(this, "LOITER_RAD") { + updateParams(); } Navigator::~Navigator() @@ -188,6 +189,13 @@ Navigator::vehicle_control_mode_update() } } +void +Navigator::params_update() +{ + parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶m_update); +} + void Navigator::task_main_trampoline(int argc, char *argv[]) { @@ -226,6 +234,7 @@ Navigator::task_main() _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)); + _param_update_sub - orb_subscribe(ORB_ID(parameter_update)); /* copy all topics first time */ vehicle_status_update(); @@ -233,6 +242,7 @@ Navigator::task_main() global_position_update(); home_position_update(); navigation_capabilities_update(); + params_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -241,7 +251,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[5]; + struct pollfd fds[6]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -254,6 +264,8 @@ Navigator::task_main() fds[3].events = POLLIN; fds[4].fd = _control_mode_sub; fds[4].events = POLLIN; + fds[5].fd = _param_update_sub; + fds[5].events = POLLIN; while (!_task_should_exit) { @@ -278,6 +290,12 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* parameters updated */ + if (fds[5].revents & POLLIN) { + params_update(); + updateParams(); + } + /* vehicle control mode updated */ if (fds[4].revents & POLLIN) { vehicle_control_mode_update(); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c new file mode 100644 index 000000000..9235f3046 --- /dev/null +++ b/src/modules/navigator/navigator_params.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 for navigator in general + * + * @author Julian Oes + */ + +#include + +#include + +/** + * Loiter radius (FW only) + * + * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). + * + * @unit meters + * @min 0.0 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); -- cgit v1.2.3