navigator: param enhancements

7 files changed, 93 insertions, 18 deletions

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 <systemlib/perf_counter.h>
 
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/parameter_update.h>
 
 #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
 	 */
@@ -176,6 +182,11 @@ private:
 	void		vehicle_control_mode_update();
 
 	/**
+	 * Update parameters
+	 */
+	void		params_update();
+
+	/**
 	 * Shim for calling task_main from task_create.
 	 */
 	static void	task_main_trampoline(int argc, char *argv[]);
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()
@@ -189,6 +190,13 @@ Navigator::vehicle_control_mode_update()
 }
 
 void
+Navigator::params_update()
+{
+	parameter_update_s param_update;
+	orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
+}
+
+void
 Navigator::task_main_trampoline(int argc, char *argv[])
 {
 	navigator::g_navigator->task_main();
@@ -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 <julian@oes.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/**
+ * 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);