Merge branch 'master' into seatbelt_multirotor

6 files changed, 796 insertions, 0 deletions

diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk
new file mode 100644
index 000000000..d04847745
--- /dev/null
+++ b/src/modules/multirotor_pos_control/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 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 multirotor position control
+#
+
+MODULE_COMMAND	= multirotor_pos_control
+
+SRCS		= multirotor_pos_control.c \
+		  multirotor_pos_control_params.c
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
new file mode 100644
index 000000000..6fe129d84
--- /dev/null
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -0,0 +1,325 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.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 multirotor_pos_control.c
+ *
+ * Multirotor position controller
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <termios.h>
+#include <time.h>
+#include <sys/prctl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_vicon_position.h>
+#include <systemlib/systemlib.h>
+#include <mavlink/mavlink_log.h>
+
+#include "multirotor_pos_control_params.h"
+
+
+static bool thread_should_exit = false;		/**< Deamon exit flag */
+static bool thread_running = false;		/**< Deamon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
+
+__EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of position controller.
+ */
+static int multirotor_pos_control_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void usage(const char *reason) {
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ * 
+ * The actual stack size should be set in the call
+ * to task_spawn().
+ */
+int multirotor_pos_control_main(int argc, char *argv[]) {
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("multirotor_pos_control already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("multirotor_pos_control",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 60,
+					 4096,
+					 multirotor_pos_control_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tmultirotor_pos_control app is running\n");
+		} else {
+			printf("\tmultirotor_pos_control app not started\n");
+		}
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
+	/* welcome user */
+	printf("[multirotor_pos_control] started\n");
+	static int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started");
+
+	/* structures */
+	struct vehicle_status_s status;
+	memset(&status, 0, sizeof(status));
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_sp));
+	struct manual_control_setpoint_s manual;
+	memset(&manual, 0, sizeof(manual));
+	struct vehicle_local_position_s local_pos;
+	memset(&local_pos, 0, sizeof(local_pos));
+	struct vehicle_local_position_setpoint_s local_pos_sp;
+	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
+
+	/* subscribe to attitude, motor setpoints and system state */
+	int param_sub = orb_subscribe(ORB_ID(parameter_update));
+	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
+	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+
+	/* publish setpoint */
+	orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
+	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+
+	bool reset_sp_alt = true;
+	bool reset_sp_pos = true;
+	hrt_abstime t_prev = 0;
+	float alt_integral = 0.0f;
+	float alt_ctl_dz = 0.2f;
+
+	thread_running = true;
+
+	struct multirotor_position_control_params params;
+	struct multirotor_position_control_param_handles params_h;
+	parameters_init(&params_h);
+	parameters_update(&params_h, &params);
+
+	int paramcheck_counter = 0;
+
+	while (!thread_should_exit) {
+		orb_copy(ORB_ID(vehicle_status), state_sub, &status);
+
+		/* check parameters at 1 Hz*/
+		paramcheck_counter++;
+		if (paramcheck_counter == 50) {
+			bool param_updated;
+			orb_check(param_sub, &param_updated);
+			if (param_updated) {
+				parameters_update(&params_h, &params);
+			}
+			paramcheck_counter = 0;
+		}
+
+		/* Check if controller should act */
+		bool act = status.flag_system_armed && (
+		   		       /* SAS modes */
+				   	   (
+				   		   status.flag_control_manual_enabled &&
+						   status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && (
+								   status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE ||
+								   status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE
+						   )
+				   	   ) ||
+				   	   /* AUTO mode */
+				   	   status.state_machine == SYSTEM_STATE_AUTO
+				   );
+
+		hrt_abstime t = hrt_absolute_time();
+		float dt;
+		if (t_prev != 0) {
+			dt = (t - t_prev) * 0.000001f;
+		} else {
+			dt = 0.0f;
+		}
+		t_prev = t;
+		if (act) {
+			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
+			orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
+			if (status.state_machine == SYSTEM_STATE_AUTO) {
+				orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
+			}
+
+			if (reset_sp_alt) {
+				reset_sp_alt = false;
+				local_pos_sp.z = local_pos.z;
+				alt_integral = manual.throttle;
+				char str[80];
+				sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle);
+				mavlink_log_info(mavlink_fd, str);
+			}
+
+			if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) {
+				reset_sp_pos = false;
+				local_pos_sp.x = local_pos.x;
+				local_pos_sp.y = local_pos.y;
+				char str[80];
+				sprintf(str, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y);
+				mavlink_log_info(mavlink_fd, str);
+			}
+
+			float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max;
+
+			if (status.flag_control_manual_enabled) {
+				/* move altitude setpoint with manual controls */
+				float alt_ctl = manual.throttle - 0.5f;
+				if (fabs(alt_ctl) < alt_ctl_dz) {
+					alt_ctl = 0.0f;
+				} else {
+					if (alt_ctl > 0.0f)
+						alt_ctl = (alt_ctl - alt_ctl_dz) / (0.5f - alt_ctl_dz);
+					else
+						alt_ctl = (alt_ctl + alt_ctl_dz) / (0.5f - alt_ctl_dz);
+					local_pos_sp.z -= alt_ctl * params.alt_rate_max * dt;
+					if (local_pos_sp.z > local_pos.z + err_linear_limit)
+						local_pos_sp.z = local_pos.z + err_linear_limit;
+					else if (local_pos_sp.z < local_pos.z - err_linear_limit)
+						local_pos_sp.z = local_pos.z - err_linear_limit;
+				}
+
+				if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) {
+					// TODO move position setpoint with manual controls
+				}
+			}
+
+			/* PID for altitude */
+			float alt_err = local_pos.z - local_pos_sp.z;
+			/* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */
+			if (alt_err > err_linear_limit) {
+				alt_err = err_linear_limit;
+			} else if (alt_err < -err_linear_limit) {
+				alt_err = -err_linear_limit;
+			}
+			/* P and D components */
+			float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d;
+			/* add I component */
+			float thrust_ctl = thrust_ctl_pd + alt_integral;
+			alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt;
+			if (thrust_ctl < params.thr_min) {
+				thrust_ctl = params.thr_min;
+			} else if (thrust_ctl > params.thr_max) {
+				thrust_ctl = params.thr_max;
+			}
+			if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) {
+				// TODO add position controller
+			} else {
+				reset_sp_pos = true;
+			}
+			att_sp.thrust = thrust_ctl;
+			att_sp.timestamp = hrt_absolute_time();
+			if (status.flag_control_manual_enabled) {
+				/* publish local position setpoint in manual mode */
+				orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+			}
+			/* publish new attitude setpoint */
+			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+		} else {
+			reset_sp_alt = true;
+			reset_sp_pos = true;
+		}
+
+		/* run at approximately 50 Hz */
+		usleep(20000);
+
+	}
+
+	printf("[multirotor_pos_control] exiting\n");
+	mavlink_log_info(mavlink_fd, "[multirotor_pos_control] exiting");
+
+	thread_running = false;
+
+	fflush(stdout);
+	return 0;
+}
+
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
new file mode 100644
index 000000000..90dd820f4
--- /dev/null
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 multirotor_pos_control_params.c
+ * 
+ * Parameters for multirotor_pos_control
+ */
+
+#include "multirotor_pos_control_params.h"
+
+/* controller parameters */
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f);
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f);
+PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.01f);
+PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f);
+
+int parameters_init(struct multirotor_position_control_param_handles *h)
+{
+	h->thr_min 	=	param_find("MPC_THR_MIN");
+	h->thr_max 	=	param_find("MPC_THR_MAX");
+	/* PID parameters */
+	h->alt_p 	=	param_find("MPC_ALT_P");
+	h->alt_i 	=	param_find("MPC_ALT_I");
+	h->alt_d 	=	param_find("MPC_ALT_D");
+	h->alt_rate_max 	=	param_find("MPC_ALT_RATE_MAX");
+	return OK;
+}
+
+int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
+{
+	param_get(h->thr_min, &(p->thr_min));
+	param_get(h->thr_max, &(p->thr_max));
+	param_get(h->alt_p, &(p->alt_p));
+	param_get(h->alt_i, &(p->alt_i));
+	param_get(h->alt_d, &(p->alt_d));
+	param_get(h->alt_rate_max, &(p->alt_rate_max));
+	return OK;
+}
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
new file mode 100644
index 000000000..849055cd1
--- /dev/null
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 multirotor_position_control_params.h
+ * 
+ * Parameters for position controller
+ */
+
+#include <systemlib/param/param.h>
+
+struct multirotor_position_control_params {
+	float thr_min;
+	float thr_max;
+	float alt_p;
+	float alt_i;
+	float alt_d;
+	float alt_rate_max;
+};
+
+struct multirotor_position_control_param_handles {
+	param_t thr_min;
+	param_t thr_max;
+	param_t alt_p;
+	param_t alt_i;
+	param_t alt_d;
+	param_t alt_rate_max;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct multirotor_position_control_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p);
diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c
new file mode 100644
index 000000000..9c53a5bf6
--- /dev/null
+++ b/src/modules/multirotor_pos_control/position_control.c
@@ -0,0 +1,235 @@
+// /****************************************************************************
+//  *
+//  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+//  *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+//  *           @author Laurens Mackay <mackayl@student.ethz.ch>
+//  *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+//  *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+//  *
+//  * 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 multirotor_position_control.c
+//  * Implementation of the position control for a multirotor VTOL
+//  */
+
+// #include <stdio.h>
+// #include <stdlib.h>
+// #include <stdio.h>
+// #include <stdint.h>
+// #include <math.h>
+// #include <stdbool.h>
+// #include <float.h>
+// #include <systemlib/pid/pid.h>
+
+// #include "multirotor_position_control.h"
+
+// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
+//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
+//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
+// {
+// 	static PID_t distance_controller;
+
+// 	static int read_ret;
+// 	static global_data_position_t position_estimated;
+
+// 	static uint16_t counter;
+
+// 	static bool initialized;
+// 	static uint16_t pm_counter;
+
+// 	static float lat_next;
+// 	static float lon_next;
+
+// 	static float pitch_current;
+
+// 	static float thrust_total;
+
+
+// 	if (initialized == false) {
+
+// 		pid_init(&distance_controller,
+// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
+// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
+// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
+// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
+// 			 PID_MODE_DERIVATIV_CALC, 150);//150
+
+// //		pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
+// //		pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
+
+// 		thrust_total = 0.0f;
+
+// 		/* Position initialization */
+// 		/* Wait for new position estimate */
+// 		do {
+// 			read_ret = read_lock_position(&position_estimated);
+// 		} while (read_ret != 0);
+
+// 		lat_next = position_estimated.lat;
+// 		lon_next = position_estimated.lon;
+
+// 		/* attitude initialization */
+// 		global_data_lock(&global_data_attitude->access_conf);
+// 		pitch_current = global_data_attitude->pitch;
+// 		global_data_unlock(&global_data_attitude->access_conf);
+
+// 		initialized = true;
+// 	}
+
+// 	/* load new parameters with 10Hz */
+// 	if (counter % 50 == 0) {
+// 		if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
+// 			/* check whether new parameters are available */
+// 			if (global_data_parameter_storage->counter > pm_counter) {
+// 				pid_set_parameters(&distance_controller,
+// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
+// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
+// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
+// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
+
+// //
+// //				pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
+// //				pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
+
+// 				pm_counter = global_data_parameter_storage->counter;
+// 				printf("Position controller changed pid parameters\n");
+// 			}
+// 		}
+
+// 		global_data_unlock(&global_data_parameter_storage->access_conf);
+// 	}
+
+
+// 	/* Wait for new position estimate */
+// 	do {
+// 		read_ret = read_lock_position(&position_estimated);
+// 	} while (read_ret != 0);
+
+// 	/* Get next waypoint */ //TODO: add local copy
+
+// 	if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
+// 		lat_next = global_data_position_setpoint->x;
+// 		lon_next = global_data_position_setpoint->y;
+// 		global_data_unlock(&global_data_position_setpoint->access_conf);
+// 	}
+
+// 	/* Get distance to waypoint */
+// 	float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
+// //	if(counter % 5 == 0)
+// //		printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
+
+// 	/* Get bearing to waypoint (direction on earth surface to next waypoint) */
+// 	float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
+
+// 	if (counter % 5 == 0)
+// 		printf("bearing: %.4f\n", bearing);
+
+// 	/* Calculate speed in direction of bearing (needed for controller) */
+// 	float speed_norm = sqrtf(position_estimated.vx  * position_estimated.vx + position_estimated.vy * position_estimated.vy);
+// //	if(counter % 5 == 0)
+// //		printf("speed_norm: %.4f\n", speed_norm);
+// 	float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
+
+// 	/* Control Thrust in bearing direction  */
+// 	float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
+// 				  CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
+
+// //	if(counter % 5 == 0)
+// //		printf("horizontal thrust: %.4f\n", horizontal_thrust);
+
+// 	/* Get total thrust (from remote for now) */
+// 	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
+// 		thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; 												//TODO: how should we use the RC_CHANNELS_FUNCTION enum?
+// 		global_data_unlock(&global_data_rc_channels->access_conf);
+// 	}
+
+// 	const float max_gas = 500.0f;
+// 	thrust_total *= max_gas / 20000.0f; //TODO: check this
+// 	thrust_total += max_gas / 2.0f;
+
+
+// 	if (horizontal_thrust > thrust_total) {
+// 		horizontal_thrust = thrust_total;
+
+// 	} else if (horizontal_thrust < -thrust_total) {
+// 		horizontal_thrust = -thrust_total;
+// 	}
+
+
+
+// 	//TODO: maybe we want to add a speed controller later...
+
+// 	/* Calclulate thrust in east and north direction */
+// 	float thrust_north = cosf(bearing) * horizontal_thrust;
+// 	float thrust_east = sinf(bearing) * horizontal_thrust;
+
+// 	if (counter % 10 == 0) {
+// 		printf("thrust north: %.4f\n", thrust_north);
+// 		printf("thrust east: %.4f\n", thrust_east);
+// 		fflush(stdout);
+// 	}
+
+// 	/* Get current attitude */
+// 	if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
+// 		pitch_current = global_data_attitude->pitch;
+// 		global_data_unlock(&global_data_attitude->access_conf);
+// 	}
+
+// 	/* Get desired pitch & roll */
+// 	float pitch_desired = 0.0f;
+// 	float roll_desired = 0.0f;
+
+// 	if (thrust_total != 0) {
+// 		float pitch_fraction = -thrust_north / thrust_total;
+// 		float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
+
+// 		if (roll_fraction < -1) {
+// 			roll_fraction = -1;
+
+// 		} else if (roll_fraction > 1) {
+// 			roll_fraction = 1;
+// 		}
+
+// //		if(counter % 5 == 0)
+// //		{
+// //			printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
+// //			fflush(stdout);
+// //		}
+
+// 		pitch_desired = asinf(pitch_fraction);
+// 		roll_desired = asinf(roll_fraction);
+// 	}
+
+// 	att_sp.roll = roll_desired;
+// 	att_sp.pitch = pitch_desired;
+
+// 	counter++;
+// }
diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/modules/multirotor_pos_control/position_control.h
new file mode 100644
index 000000000..2144ebc34
--- /dev/null
+++ b/src/modules/multirotor_pos_control/position_control.h
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Laurens Mackay <mackayl@student.ethz.ch>
+ *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *
+ * 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 multirotor_position_control.h
+ * Definition of the position control for a multirotor VTOL
+ */
+
+// #ifndef POSITION_CONTROL_H_
+// #define POSITION_CONTROL_H_
+
+// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
+//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
+//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
+
+// #endif /* POSITION_CONTROL_H_ */