navigator: added offboard mode

5 files changed, 221 insertions, 1 deletions

diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index a1e109030..637eaae59 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -46,6 +46,7 @@ SRCS		= navigator_main.cpp \
 		  loiter.cpp \
 		  rtl.cpp \
 		  rtl_params.c \
+		  offboard.cpp \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
 		  geofence_params.c
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index dadd15527..e6ff213bd 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -56,13 +56,14 @@
 #include "mission.h"
 #include "loiter.h"
 #include "rtl.h"
+#include "offboard.h"
 #include "geofence.h"
 
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  * Currently: mission, loiter, and rtl
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 3
+#define NAVIGATOR_MODE_ARRAY_SIZE 4
 
 class Navigator : public control::SuperBlock
 {
@@ -108,10 +109,13 @@ public:
 	 * Getters
 	 */
 	struct vehicle_status_s*	    get_vstatus() { return &_vstatus; }
+	struct vehicle_control_mode_s*	    get_control_mode() { return &_control_mode; }
 	struct vehicle_global_position_s*   get_global_position() { return &_global_pos; }
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
+
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
+	int		get_offboard_control_sp_sub() { return _offboard_control_sp_sub; }
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_is_in_loiter() { return _is_in_loiter; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
@@ -129,6 +133,7 @@ private:
 	int		_home_pos_sub;			/**< home position subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
+	int		_offboard_control_sp_sub;	/*** offboard control subscription */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 	int		_onboard_mission_sub;		/**< onboard mission subscription */
 	int		_offboard_mission_sub;		/**< offboard mission subscription */
@@ -158,6 +163,7 @@ private:
 	Mission		_mission;			/**< class that handles the missions */
 	Loiter		_loiter;			/**< class that handles loiter */
 	RTL 		_rtl;				/**< class that handles RTL */
+	Offboard	_offboard;			/**< class that handles offboard */
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 8ab50f0e1..8ba835a87 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -67,6 +67,7 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
 #include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/offboard_control_setpoint.h>
 
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
@@ -100,6 +101,7 @@ Navigator::Navigator() :
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
 	_capabilities_sub(-1),
+	_offboard_control_sp_sub(-1),
 	_control_mode_sub(-1),
 	_onboard_mission_sub(-1),
 	_offboard_mission_sub(-1),
@@ -121,6 +123,7 @@ Navigator::Navigator() :
 	_mission(this, "MIS"),
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
+	_offboard(this, "OFF"),
 	_update_triplet(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
@@ -129,6 +132,7 @@ Navigator::Navigator() :
 	_navigation_mode_array[0] = &_mission;
 	_navigation_mode_array[1] = &_loiter;
 	_navigation_mode_array[2] = &_rtl;
+	_navigation_mode_array[3] = &_offboard;
 
 	updateParams();
 }
@@ -241,6 +245,7 @@ Navigator::task_main()
 	_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));
+	_offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
 
 	/* copy all topics first time */
 	vehicle_status_update();
@@ -364,6 +369,8 @@ Navigator::task_main()
 			case NAVIGATION_STATE_LAND:
 			case NAVIGATION_STATE_TERMINATION:
 			case NAVIGATION_STATE_OFFBOARD:
+				_navigation_mode = &_offboard;
+				break;
 			default:
 				_navigation_mode = nullptr;
 				_is_in_loiter = false;
diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp
new file mode 100644
index 000000000..b0cbb1aa1
--- /dev/null
+++ b/src/modules/navigator/offboard.cpp
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   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 offboard.cpp
+ *
+ * Helper class for offboard commands
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+
+#include "navigator.h"
+#include "offboard.h"
+
+Offboard::Offboard(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
+	_offboard_control_sp({0})
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+Offboard::~Offboard()
+{
+}
+
+bool
+Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	bool updated;
+	orb_check(_navigator->get_offboard_control_sp_sub(), &updated);
+	if (updated) {
+		update_offboard_control_setpoint();
+	}
+
+	bool changed = false;
+
+	/* copy offboard setpoints to the corresponding topics */
+	if (_navigator->get_control_mode()->flag_control_position_enabled
+	    && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) {
+		/* position control */
+		pos_sp_triplet->current.x = _offboard_control_sp.p1;
+		pos_sp_triplet->current.y = _offboard_control_sp.p2;
+		pos_sp_triplet->current.yaw = _offboard_control_sp.p3;
+		pos_sp_triplet->current.z = -_offboard_control_sp.p4;
+
+		pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
+		pos_sp_triplet->current.valid = true;
+		pos_sp_triplet->current.position_valid = true;
+
+		changed = true;
+
+	} else if (_navigator->get_control_mode()->flag_control_velocity_enabled
+	           && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) {
+		/* velocity control */
+		pos_sp_triplet->current.vx = _offboard_control_sp.p2;
+		pos_sp_triplet->current.vy = _offboard_control_sp.p1;
+		pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3*10.0f;
+		pos_sp_triplet->current.vz = _offboard_control_sp.p4;
+
+		pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
+		pos_sp_triplet->current.valid = true;
+		pos_sp_triplet->current.velocity_valid = true;
+
+		changed = true;
+	}
+
+	return changed;
+}
+
+void
+Offboard::on_inactive()
+{
+}
+
+void
+Offboard::update_offboard_control_setpoint()
+{
+	orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp);
+
+}
diff --git a/src/modules/navigator/offboard.h b/src/modules/navigator/offboard.h
new file mode 100644
index 000000000..4d415dddb
--- /dev/null
+++ b/src/modules/navigator/offboard.h
@@ -0,0 +1,82 @@
+/***************************************************************************
+ *
+ *   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 offboard.h
+ *
+ * Helper class for offboard commands
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_OFFBOARD_H
+#define NAVIGATOR_OFFBOARD_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+
+#include "navigator_mode.h"
+
+class Navigator;
+
+class Offboard : public NavigatorMode
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Offboard(Navigator *navigator, const char *name);
+
+	/**
+	 * Destructor
+	 */
+	~Offboard();
+
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	virtual void on_inactive();
+
+	/**
+	 * This function is called while the mode is active
+	 */
+	virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
+private:
+	void update_offboard_control_setpoint();
+
+	struct offboard_control_setpoint_s _offboard_control_sp;
+};
+
+#endif