initial port of multiplatform version of mc_pos_control

CMakeLists.txt

@@ -69,6 +69,11 @@ add_message_files(
   actuator_armed.msg
   parameter_update.msg
   vehicle_status.msg
+  vehicle_local_position.msg
+  position_setpoint.msg
+  position_setpoint_triplet.msg
+  vehicle_local_position_setpoint.msg
+  vehicle_global_velocity_setpoint.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -180,6 +185,16 @@ target_link_libraries(mc_att_control
   px4
 )
 
+## MC Position Control
+add_executable(mc_pos_control
+	src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
+	src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp)
+add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mc_pos_control
+  ${catkin_LIBRARIES}
+  px4
+)
+
 ## Attitude Estimator dummy
 add_executable(attitude_estimator
 	src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -14,7 +14,13 @@ else
 	# try the multiplatform version
 	mc_att_control_m start
 fi
-mc_pos_control start
+
+if mc_pos_control start
+then
+else
+	# try the multiplatform version
+	mc_pos_control_m start
+fi
 
 #
 # Start Land Detector

makefiles/config_px4fmu-v2_multiplatform.mk

@@ -89,7 +89,8 @@ MODULES		+= modules/position_estimator_inav
 MODULES		+= modules/mc_att_control_multiplatform
 MODULES		+= examples/subscriber
 MODULES		+= examples/publisher
-MODULES		+= modules/mc_pos_control
+# MODULES		+= modules/mc_pos_control
+MODULES		+= modules/mc_pos_control_multiplatform
 MODULES 	+= modules/vtol_att_control
 
 #

src/modules/mc_pos_control_multiplatform/mc_pos_control.h

@@ -0,0 +1,219 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 - 2015 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 mc_pos_control.h
+ * Multicopter position controller.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#pragma once
+
+#include <px4.h>
+#include <cstdio>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+// #include <poll.h>
+// #include <drivers/drv_hrt.h>
+// #include <arch/board/board.h>
+// #include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+// #include <mavlink/mavlink_log.h>
+
+using namespace px4;
+
+class MulticopterPositionControl
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterPositionControl();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~MulticopterPositionControl();
+
+	/* Callbacks for topics */
+	void handle_vehicle_attitude(const px4_vehicle_attitude &msg);
+	void handle_parameter_update(const px4_parameter_update &msg);
+
+	void spin() { _n.spin(); }
+
+protected:
+	const float alt_ctl_dz = 0.2f;
+
+	bool		_task_should_exit;		/**< if true, task should exit */
+	int		_control_task;			/**< task handle for task */
+	int		_mavlink_fd;			/**< mavlink fd */
+
+	Publisher<px4_vehicle_attitude_setpoint>	*_att_sp_pub;			/**< attitude setpoint publication */
+	Publisher<px4_vehicle_local_position_setpoint>	*_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
+	Publisher<px4_vehicle_global_velocity_setpoint>		*_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
+
+
+	Subscriber<px4_vehicle_attitude> *_att;			    /**< vehicle attitude */
+	Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp;	    /**< vehicle attitude setpoint */
+	Subscriber<px4_vehicle_control_mode> *_control_mode;	    /**< vehicle control mode */
+	Subscriber<px4_parameter_update> *_parameter_update;	    /**< parameter update */
+	Subscriber<px4_manual_control_setpoint> *_manual_control_sp;   /**< manual control setpoint */
+	Subscriber<px4_actuator_armed> *_armed;			    /**< actuator arming status */
+	Subscriber<px4_vehicle_local_position> *_local_pos;			    /**< local position */
+	Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet;			    /**< local position */
+	Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp;			    /**< local position */
+	Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp;			    /**< local position */
+
+	px4_vehicle_attitude_setpoint _att_sp_msg;
+	px4_vehicle_local_position_setpoint _local_pos_sp_msg;
+	px4_vehicle_global_velocity_setpoint _global_vel_sp_msg;
+
+	px4::NodeHandle _n;
+
+
+	struct {
+		px4_param_t thr_min;
+		px4_param_t thr_max;
+		px4_param_t z_p;
+		px4_param_t z_vel_p;
+		px4_param_t z_vel_i;
+		px4_param_t z_vel_d;
+		px4_param_t z_vel_max;
+		px4_param_t z_ff;
+		px4_param_t xy_p;
+		px4_param_t xy_vel_p;
+		px4_param_t xy_vel_i;
+		px4_param_t xy_vel_d;
+		px4_param_t xy_vel_max;
+		px4_param_t xy_ff;
+		px4_param_t tilt_max_air;
+		px4_param_t land_speed;
+		px4_param_t tilt_max_land;
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	struct {
+		float thr_min;
+		float thr_max;
+		float tilt_max_air;
+		float land_speed;
+		float tilt_max_land;
+
+		math::Vector<3> pos_p;
+		math::Vector<3> vel_p;
+		math::Vector<3> vel_i;
+		math::Vector<3> vel_d;
+		math::Vector<3> vel_ff;
+		math::Vector<3> vel_max;
+		math::Vector<3> sp_offs_max;
+	}		_params;
+
+	struct map_projection_reference_s _ref_pos;
+	float _ref_alt;
+	uint64_t _ref_timestamp;
+
+	bool _reset_pos_sp;
+	bool _reset_alt_sp;
+	bool _mode_auto;
+
+	math::Vector<3> _pos;
+	math::Vector<3> _pos_sp;
+	math::Vector<3> _vel;
+	math::Vector<3> _vel_sp;
+	math::Vector<3> _vel_prev;			/**< velocity on previous step */
+	math::Vector<3> _vel_ff;
+	math::Vector<3> _sp_move_rate;
+
+	math::Vector<3> _thrust_int;
+	math::Matrix<3, 3> _R;
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int			parameters_update();
+
+	/**
+	 * Update control outputs
+	 */
+	void		control_update();
+
+	static float	scale_control(float ctl, float end, float dz);
+
+	/**
+	 * Update reference for local position projection
+	 */
+	void		update_ref();
+	/**
+	 * Reset position setpoint to current position
+	 */
+	void		reset_pos_sp();
+
+	/**
+	 * Reset altitude setpoint to current altitude
+	 */
+	void		reset_alt_sp();
+
+	/**
+	 * Check if position setpoint is too far from current position and adjust it if needed.
+	 */
+	void		limit_pos_sp_offset();
+
+	/**
+	 * Set position setpoint using manual control
+	 */
+	void		control_manual(float dt);
+
+	/**
+	 * Set position setpoint using offboard control
+	 */
+	void		control_offboard(float dt);
+
+	bool		cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
+					const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res);
+
+	/**
+	 * Set position setpoint for AUTO
+	 */
+	void		control_auto(float dt);
+
+	/**
+	 * Select between barometric and global (AMSL) altitudes
+	 */
+	void		select_alt(bool global);
+};

src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 - 2015 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 mc_pos_control_main.cpp
+ * Multicopter position controller.
+ *
+ * The controller has two loops: P loop for position error and PID loop for velocity error.
+ * Output of velocity controller is thrust vector that splitted to thrust direction
+ * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "mc_pos_control.h"
+
+bool thread_running = false;     /**< Deamon status flag */
+
+int main(int argc, char **argv)
+{
+	px4::init(argc, argv, "mc_pos_control_m");
+
+	PX4_INFO("starting");
+	MulticopterPositionControl posctl;
+	thread_running = true;
+	posctl.spin();
+
+	PX4_INFO("exiting.");
+	thread_running = false;
+	return 0;
+}

src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c

@@ -0,0 +1,212 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Anton Babushkin <anton.babushkin@me.com>
+ *
+ * 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) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_pos_control_params.c
+ * Multicopter position controller parameters.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <px4_defines.h>
+#include "mc_pos_control_params.h"
+#include <systemlib/param/param.h>
+
+/**
+ * Minimum thrust
+ *
+ * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN);
+
+/**
+ * Maximum thrust
+ *
+ * Limit max allowed thrust.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX);
+
+/**
+ * Proportional gain for vertical position error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_P);
+
+/**
+ * Proportional gain for vertical velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P);
+
+/**
+ * Integral gain for vertical velocity error
+ *
+ * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I);
+
+/**
+ * Differential gain for vertical velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D);
+
+/**
+ * Maximum vertical velocity
+ *
+ * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL).
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX);
+
+/**
+ * Vertical velocity feed forward
+ *
+ * Feed forward weight for altitude control in stabilized modes (ALTCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF);
+
+/**
+ * Proportional gain for horizontal position error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_P);
+
+/**
+ * Proportional gain for horizontal velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P);
+
+/**
+ * Integral gain for horizontal velocity error
+ *
+ * Non-zero value allows to resist wind.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I);
+
+/**
+ * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D);
+
+/**
+ * Maximum horizontal velocity
+ *
+ * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL).
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX);
+
+/**
+ * Horizontal velocity feed forward
+ *
+ * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF);
+
+/**
+ * Maximum tilt angle in air
+ *
+ * Limits maximum tilt in AUTO and POSCTRL modes during flight.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR);
+
+/**
+ * Maximum tilt during landing
+ *
+ * Limits maximum tilt angle on landing.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND);
+
+/**
+ * Landing descend rate
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED);
+

src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h

@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Anton Babushkin <anton.babushkin@me.com>
+ *
+ * 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 mc_pos_control_params.h
+ * Multicopter position controller parameters.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#pragma once
+
+#define PARAM_MPC_THR_MIN_DEFAULT 0.1f
+#define PARAM_MPC_THR_MAX_DEFAULT 1.0f
+#define PARAM_MPC_Z_P_DEFAULT 1.0f
+#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f
+#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f
+#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f
+#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPC_Z_FF_DEFAULT 0.5f
+#define PARAM_MPC_XY_P_DEFAULT 1.0f
+#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f
+#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f
+#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f
+#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPC_XY_FF_DEFAULT 0.5f
+#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f
+#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f
+#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f
+

src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 - 2015 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 mc_pos_control_m_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]);
+int mc_pos_control_m_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: mc_pos_control_m {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("mc_pos_control_m",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       3000,
+				       main,
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}

src/modules/mc_pos_control_multiplatform/module.mk

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Build multicopter position controller
+#
+
+MODULE_COMMAND	= mc_pos_control_m
+
+SRCS		= mc_pos_control_main.cpp \
+		  mc_pos_control_start_nuttx.cpp \
+		  mc_pos_control.cpp \
+		  mc_pos_control_params.c

src/platforms/px4_includes.h

@@ -62,6 +62,10 @@
 #include <px4_actuator_armed.h>
 #include <px4_parameter_update.h>
 #include <px4_vehicle_status.h>
+#include <px4_vehicle_local_position_setpoint.h>
+#include <px4_vehicle_global_velocity_setpoint.h>
+#include <px4_vehicle_local_position.h>
+#include <px4_position_setpoint_triplet.h>
 #endif
 
 #else
@@ -85,6 +89,10 @@
 #include <platforms/nuttx/px4_messages/px4_actuator_armed.h>
 #include <platforms/nuttx/px4_messages/px4_parameter_update.h>
 #include <platforms/nuttx/px4_messages/px4_vehicle_status.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_local_position_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_global_velocity_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_local_position.h>
+#include <platforms/nuttx/px4_messages/px4_position_setpoint_triplet.h>
 #endif
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>