mc_att_control_vector fix, multirotor_attitude_control and multirotor_pos_control removed

8 files changed, 0 insertions, 1649 deletions

diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
index ef6a46810..5f436fb71 100644
--- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
+++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
@@ -523,16 +523,6 @@ MulticopterAttitudeControl::task_main()
 				}
 
 			} else {
-				if (!_control_mode.flag_control_auto_enabled) {
-					/* no control, try to stay on place */
-					if (!_control_mode.flag_control_velocity_enabled) {
-						/* no velocity control, reset attitude setpoint */
-						_att_sp.roll_body = 0.0f;
-						_att_sp.pitch_body = 0.0f;
-						publish_att_sp = true;
-					}
-				}
-
 				/* reset yaw setpoint after non-manual control */
 				reset_yaw_sp = true;
 			}
diff --git a/src/modules/multirotor_att_control/module.mk b/src/modules/multirotor_att_control/module.mk
deleted file mode 100755
index 7569e1c7e..000000000
--- a/src/modules/multirotor_att_control/module.mk
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   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 attitude controller
-#
-
-MODULE_COMMAND	= multirotor_att_control
-
-SRCS		= multirotor_att_control_main.c \
-		  multirotor_attitude_control.c \
-		  multirotor_rate_control.c
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
deleted file mode 100644
index 111e9197f..000000000
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ /dev/null
@@ -1,465 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *           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 multirotor_att_control_main.c
- *
- * Implementation of multirotor attitude control main loop.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- */
-
-#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 <getopt.h>
-#include <time.h>
-#include <math.h>
-#include <poll.h>
-#include <sys/prctl.h>
-#include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
-#include <drivers/drv_gyro.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/offboard_control_setpoint.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-
-#include <systemlib/perf_counter.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/param/param.h>
-
-#include "multirotor_attitude_control.h"
-#include "multirotor_rate_control.h"
-
-__EXPORT int multirotor_att_control_main(int argc, char *argv[]);
-
-static bool thread_should_exit;
-static int mc_task;
-static bool motor_test_mode = false;
-static const float min_takeoff_throttle = 0.3f;
-static const float yaw_deadzone = 0.01f;
-
-static int
-mc_thread_main(int argc, char *argv[])
-{
-	/* declare and safely initialize all structs */
-	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 offboard_control_setpoint_s offboard_sp;
-	memset(&offboard_sp, 0, sizeof(offboard_sp));
-	struct vehicle_control_mode_s control_mode;
-	memset(&control_mode, 0, sizeof(control_mode));
-	struct manual_control_setpoint_s manual;
-	memset(&manual, 0, sizeof(manual));
-	struct sensor_combined_s sensor;
-	memset(&sensor, 0, sizeof(sensor));
-	struct vehicle_rates_setpoint_s rates_sp;
-	memset(&rates_sp, 0, sizeof(rates_sp));
-	struct vehicle_status_s status;
-	memset(&status, 0, sizeof(status));
-	struct actuator_controls_s actuators;
-	memset(&actuators, 0, sizeof(actuators));
-
-	/* subscribe */
-	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
-	int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
-	int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-
-	/* publish actuator controls */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
-		actuators.control[i] = 0.0f;
-	}
-
-	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
-	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
-	orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
-
-	/* register the perf counter */
-	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
-	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval");
-	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
-
-	warnx("starting");
-
-	/* store last control mode to detect mode switches */
-	bool control_yaw_position = true;
-	bool reset_yaw_sp = true;
-
-	struct pollfd fds[1] = {
-		{ .fd = vehicle_attitude_sub, .events = POLLIN },
-	};
-
-	while (!thread_should_exit) {
-
-		/* wait for a sensor update, check for exit condition every 500 ms */
-		int ret = poll(fds, 1, 500);
-
-		if (ret < 0) {
-			/* poll error, count it in perf */
-			perf_count(mc_err_perf);
-
-		} else if (ret > 0) {
-			/* only run controller if attitude changed */
-			perf_begin(mc_loop_perf);
-
-			/* attitude */
-			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-
-			bool updated;
-
-			/* parameters */
-			orb_check(parameter_update_sub, &updated);
-
-			if (updated) {
-				struct parameter_update_s update;
-				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
-				/* update parameters */
-			}
-
-			/* control mode */
-			orb_check(vehicle_control_mode_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode);
-			}
-
-			/* manual control setpoint */
-			orb_check(manual_control_setpoint_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
-			}
-
-			/* attitude setpoint */
-			orb_check(vehicle_attitude_setpoint_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
-			}
-
-			/* offboard control setpoint */
-			orb_check(offboard_control_setpoint_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp);
-			}
-
-			/* vehicle status */
-			orb_check(vehicle_status_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status);
-			}
-
-			/* sensors */
-			orb_check(sensor_combined_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
-			}
-
-			/* set flag to safe value */
-			control_yaw_position = true;
-
-			/* reset yaw setpoint if not armed */
-			if (!control_mode.flag_armed) {
-				reset_yaw_sp = true;
-			}
-
-			/* define which input is the dominating control input */
-			if (control_mode.flag_control_offboard_enabled) {
-				/* offboard inputs */
-				if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
-					rates_sp.roll = offboard_sp.p1;
-					rates_sp.pitch = offboard_sp.p2;
-					rates_sp.yaw = offboard_sp.p3;
-					rates_sp.thrust = offboard_sp.p4;
-					rates_sp.timestamp = hrt_absolute_time();
-					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-
-				} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
-					att_sp.roll_body = offboard_sp.p1;
-					att_sp.pitch_body = offboard_sp.p2;
-					att_sp.yaw_body = offboard_sp.p3;
-					att_sp.thrust = offboard_sp.p4;
-					att_sp.timestamp = hrt_absolute_time();
-					/* publish the result to the vehicle actuators */
-					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-				}
-
-				/* reset yaw setpoint after offboard control */
-				reset_yaw_sp = true;
-
-			} else if (control_mode.flag_control_manual_enabled) {
-				/* manual input */
-				if (control_mode.flag_control_attitude_enabled) {
-					/* control attitude, update attitude setpoint depending on mode */
-					if (att_sp.thrust < 0.1f) {
-						/* no thrust, don't try to control yaw */
-						rates_sp.yaw = 0.0f;
-						control_yaw_position = false;
-
-						if (status.condition_landed) {
-							/* reset yaw setpoint if on ground */
-							reset_yaw_sp = true;
-						}
-
-					} else {
-						/* only move yaw setpoint if manual input is != 0 */
-						if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
-							/* control yaw rate */
-							control_yaw_position = false;
-							rates_sp.yaw = manual.yaw;
-							reset_yaw_sp = true;	// has no effect on control, just for beautiful log
-
-						} else {
-							control_yaw_position = true;
-						}
-					}
-
-					if (!control_mode.flag_control_velocity_enabled) {
-						/* update attitude setpoint if not in position control mode */
-						att_sp.roll_body = manual.roll;
-						att_sp.pitch_body = manual.pitch;
-
-						if (!control_mode.flag_control_climb_rate_enabled) {
-							/* pass throttle directly if not in altitude control mode */
-							att_sp.thrust = manual.throttle;
-						}
-					}
-
-					/* reset yaw setpint to current position if needed */
-					if (reset_yaw_sp) {
-						att_sp.yaw_body = att.yaw;
-						reset_yaw_sp = false;
-					}
-
-					if (motor_test_mode) {
-						printf("testmode");
-						att_sp.roll_body = 0.0f;
-						att_sp.pitch_body = 0.0f;
-						att_sp.yaw_body = 0.0f;
-						att_sp.thrust = 0.1f;
-					}
-
-					att_sp.timestamp = hrt_absolute_time();
-
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-
-				} else {
-					/* manual rate inputs (ACRO), from RC control or joystick */
-					if (control_mode.flag_control_rates_enabled) {
-						rates_sp.roll = manual.roll;
-						rates_sp.pitch = manual.pitch;
-						rates_sp.yaw = manual.yaw;
-						rates_sp.thrust = manual.throttle;
-						rates_sp.timestamp = hrt_absolute_time();
-					}
-
-					/* reset yaw setpoint after ACRO */
-					reset_yaw_sp = true;
-				}
-
-			} else {
-				if (!control_mode.flag_control_attitude_enabled) {
-					/* no control, try to stay on place */
-					if (!control_mode.flag_control_velocity_enabled) {
-						/* no velocity control, reset attitude setpoint */
-						att_sp.roll_body = 0.0f;
-						att_sp.pitch_body = 0.0f;
-						att_sp.timestamp = hrt_absolute_time();
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-					}
-				}
-
-				/* reset yaw setpoint after non-manual control */
-				reset_yaw_sp = true;
-			}
-
-			/* check if we should we reset integrals */
-			bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f;	// TODO use landed status instead of throttle
-
-			/* run attitude controller if needed */
-			if (control_mode.flag_control_attitude_enabled) {
-				multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
-				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-			}
-
-			/* measure in what intervals the controller runs */
-			perf_count(mc_interval_perf);
-
-			/* run rates controller if needed */
-			if (control_mode.flag_control_rates_enabled) {
-				/* get current rate setpoint */
-				bool rates_sp_updated = false;
-				orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated);
-
-				if (rates_sp_updated) {
-					orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp);
-				}
-
-				/* apply controller */
-				float rates[3];
-				rates[0] = att.rollspeed;
-				rates[1] = att.pitchspeed;
-				rates[2] = att.yawspeed;
-				multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
-
-			} else {
-				/* rates controller disabled, set actuators to zero for safety */
-				actuators.control[0] = 0.0f;
-				actuators.control[1] = 0.0f;
-				actuators.control[2] = 0.0f;
-				actuators.control[3] = 0.0f;
-			}
-
-			/* fill in manual control values */
-			actuators.control[4] = manual.flaps;
-			actuators.control[5] = manual.aux1;
-			actuators.control[6] = manual.aux2;
-			actuators.control[7] = manual.aux3;
-
-			actuators.timestamp = hrt_absolute_time();
-			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-
-			perf_end(mc_loop_perf);
-		}
-	}
-
-	warnx("stopping, disarming motors");
-
-	/* kill all outputs */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
-		actuators.control[i] = 0.0f;
-
-	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-
-	close(vehicle_attitude_sub);
-	close(vehicle_control_mode_sub);
-	close(manual_control_setpoint_sub);
-	close(actuator_pub);
-	close(att_sp_pub);
-
-	perf_print_counter(mc_loop_perf);
-	perf_free(mc_loop_perf);
-
-	fflush(stdout);
-	exit(0);
-}
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|status|stop}\n");
-	fprintf(stderr, "    <mode> is 'rates' or 'attitude'\n");
-	fprintf(stderr, "    -t enables motor test mode with 10%% thrust\n");
-	exit(1);
-}
-
-int multirotor_att_control_main(int argc, char *argv[])
-{
-	int	ch;
-	unsigned int optioncount = 0;
-
-	while ((ch = getopt(argc, argv, "tm:")) != EOF) {
-		switch (ch) {
-		case 't':
-			motor_test_mode = true;
-			optioncount += 1;
-			break;
-
-		case ':':
-			usage("missing parameter");
-			break;
-
-		default:
-			fprintf(stderr, "option: -%c\n", ch);
-			usage("unrecognized option");
-			break;
-		}
-	}
-
-	argc -= optioncount;
-	//argv += optioncount;
-
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1 + optioncount], "start")) {
-
-		thread_should_exit = false;
-		mc_task = task_spawn_cmd("multirotor_att_control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 15,
-					 2048,
-					 mc_thread_main,
-					 NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1 + optioncount], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
deleted file mode 100644
index b7df0433a..000000000
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ /dev/null
@@ -1,254 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@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_attitude_control.c
- *
- * Implementation of attitude controller for multirotors.
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include "multirotor_attitude_control.h"
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <float.h>
-#include <math.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/param/param.h>
-#include <drivers/drv_hrt.h>
-
-PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f);
-PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
-PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
-//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
-
-PARAM_DEFINE_FLOAT(MC_ATT_P, 6.8f);
-PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
-//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
-
-//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
-
-struct mc_att_control_params {
-	float yaw_p;
-	float yaw_i;
-	float yaw_d;
-	//float yaw_awu;
-	//float yaw_lim;
-
-	float att_p;
-	float att_i;
-	float att_d;
-	//float att_awu;
-	//float att_lim;
-
-	//float att_xoff;
-	//float att_yoff;
-};
-
-struct mc_att_control_param_handles {
-	param_t yaw_p;
-	param_t yaw_i;
-	param_t yaw_d;
-	//param_t yaw_awu;
-	//param_t yaw_lim;
-
-	param_t att_p;
-	param_t att_i;
-	param_t att_d;
-	//param_t att_awu;
-	//param_t att_lim;
-
-	//param_t att_xoff;
-	//param_t att_yoff;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-static int parameters_init(struct mc_att_control_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p);
-
-
-static int parameters_init(struct mc_att_control_param_handles *h)
-{
-	/* PID parameters */
-	h->yaw_p 	=	param_find("MC_YAWPOS_P");
-	h->yaw_i 	=	param_find("MC_YAWPOS_I");
-	h->yaw_d 	=	param_find("MC_YAWPOS_D");
-	//h->yaw_awu 	=	param_find("MC_YAWPOS_AWU");
-	//h->yaw_lim 	=	param_find("MC_YAWPOS_LIM");
-
-	h->att_p 	= 	param_find("MC_ATT_P");
-	h->att_i 	= 	param_find("MC_ATT_I");
-	h->att_d 	= 	param_find("MC_ATT_D");
-	//h->att_awu 	= 	param_find("MC_ATT_AWU");
-	//h->att_lim 	= 	param_find("MC_ATT_LIM");
-
-	//h->att_xoff 	= 	param_find("MC_ATT_XOFF");
-	//h->att_yoff 	= 	param_find("MC_ATT_YOFF");
-
-	return OK;
-}
-
-static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p)
-{
-	param_get(h->yaw_p, &(p->yaw_p));
-	param_get(h->yaw_i, &(p->yaw_i));
-	param_get(h->yaw_d, &(p->yaw_d));
-	//param_get(h->yaw_awu, &(p->yaw_awu));
-	//param_get(h->yaw_lim, &(p->yaw_lim));
-
-	param_get(h->att_p, &(p->att_p));
-	param_get(h->att_i, &(p->att_i));
-	param_get(h->att_d, &(p->att_d));
-	//param_get(h->att_awu, &(p->att_awu));
-	//param_get(h->att_lim, &(p->att_lim));
-
-	//param_get(h->att_xoff, &(p->att_xoff));
-	//param_get(h->att_yoff, &(p->att_yoff));
-
-	return OK;
-}
-
-void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral)
-{
-	static uint64_t last_run = 0;
-	static uint64_t last_input = 0;
-	float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-	last_run = hrt_absolute_time();
-
-	if (last_input != att_sp->timestamp) {
-		last_input = att_sp->timestamp;
-	}
-
-	static int motor_skip_counter = 0;
-
-	static PID_t pitch_controller;
-	static PID_t roll_controller;
-
-	static struct mc_att_control_params p;
-	static struct mc_att_control_param_handles h;
-
-	static bool initialized = false;
-
-	static float yaw_error;
-
-	/* initialize the pid controllers when the function is called for the first time */
-	if (initialized == false) {
-		parameters_init(&h);
-		parameters_update(&h, &p);
-
-		pid_init(&pitch_controller, PID_MODE_DERIVATIV_SET, 0.0f);
-		pid_init(&roll_controller, PID_MODE_DERIVATIV_SET, 0.0f);
-
-		initialized = true;
-	}
-
-	/* load new parameters with lower rate */
-	if (motor_skip_counter % 500 == 0) {
-		/* update parameters from storage */
-		parameters_update(&h, &p);
-
-		/* apply parameters */
-		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
-		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
-	}
-
-	/* reset integrals if needed */
-	if (reset_integral) {
-		pid_reset_integral(&pitch_controller);
-		pid_reset_integral(&roll_controller);
-		//TODO pid_reset_integral(&yaw_controller);
-	}
-
-	/* calculate current control outputs */
-
-	/* control pitch (forward) output */
-	rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
-					att->pitch, att->pitchspeed, deltaT);
-
-	/* control roll (left/right) output */
-	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
-				       att->roll, att->rollspeed, deltaT);
-
-	if (control_yaw_position) {
-		/* control yaw rate */
-		// TODO use pid lib
-
-		/* positive error: rotate to right, negative error, rotate to left (NED frame) */
-		// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
-
-		yaw_error = att_sp->yaw_body - att->yaw;
-
-		if (yaw_error > M_PI_F) {
-			yaw_error -= M_TWOPI_F;
-
-		} else if (yaw_error < -M_PI_F) {
-			yaw_error += M_TWOPI_F;
-		}
-
-		rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
-	}
-
-	rates_sp->thrust = att_sp->thrust;
-    //need to update the timestamp now that we've touched rates_sp
-    rates_sp->timestamp = hrt_absolute_time();
-
-	motor_skip_counter++;
-}
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h
deleted file mode 100644
index 431a435f7..000000000
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@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_attitude_control.h
- *
- * Definition of attitude controller for multirotors.
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_
-#define MULTIROTOR_ATTITUDE_CONTROL_H_
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-
-void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral);
-
-#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
deleted file mode 100644
index eb41bf93e..000000000
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ /dev/null
@@ -1,196 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
- *           Julian Oes <joes@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_rate_control.c
- *
- * Implementation of rate controller for multirotors.
- *
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#include "multirotor_rate_control.h"
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <float.h>
-#include <math.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
-
-PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f);
-
-PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
-
-struct mc_rate_control_params {
-
-	float yawrate_p;
-	float yawrate_d;
-	float yawrate_i;
-
-	float attrate_p;
-	float attrate_d;
-	float attrate_i;
-
-	float rate_lim;
-};
-
-struct mc_rate_control_param_handles {
-
-	param_t yawrate_p;
-	param_t yawrate_i;
-	param_t yawrate_d;
-
-	param_t attrate_p;
-	param_t attrate_i;
-	param_t attrate_d;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-static int parameters_init(struct mc_rate_control_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p);
-
-
-static int parameters_init(struct mc_rate_control_param_handles *h)
-{
-	/* PID parameters */
-	h->yawrate_p 	=	param_find("MC_YAWRATE_P");
-	h->yawrate_i 	=	param_find("MC_YAWRATE_I");
-	h->yawrate_d 	=	param_find("MC_YAWRATE_D");
-
-	h->attrate_p 	= 	param_find("MC_ATTRATE_P");
-	h->attrate_i 	= 	param_find("MC_ATTRATE_I");
-	h->attrate_d 	= 	param_find("MC_ATTRATE_D");
-
-	return OK;
-}
-
-static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p)
-{
-	param_get(h->yawrate_p, &(p->yawrate_p));
-	param_get(h->yawrate_i, &(p->yawrate_i));
-	param_get(h->yawrate_d, &(p->yawrate_d));
-
-	param_get(h->attrate_p, &(p->attrate_p));
-	param_get(h->attrate_i, &(p->attrate_i));
-	param_get(h->attrate_d, &(p->attrate_d));
-
-	return OK;
-}
-
-void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
-{
-	static uint64_t last_run = 0;
-	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-	static uint64_t last_input = 0;
-
-	if (last_input != rate_sp->timestamp) {
-		last_input = rate_sp->timestamp;
-	}
-
-	last_run = hrt_absolute_time();
-
-	static int motor_skip_counter = 0;
-
-	static PID_t pitch_rate_controller;
-	static PID_t roll_rate_controller;
-	static PID_t yaw_rate_controller;
-
-	static struct mc_rate_control_params p;
-	static struct mc_rate_control_param_handles h;
-
-	static bool initialized = false;
-
-	/* initialize the pid controllers when the function is called for the first time */
-	if (initialized == false) {
-		parameters_init(&h);
-		parameters_update(&h, &p);
-		initialized = true;
-
-		pid_init(&pitch_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
-		pid_init(&roll_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
-		pid_init(&yaw_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
-	}
-
-	/* load new parameters with lower rate */
-	if (motor_skip_counter % 2500 == 0) {
-		/* update parameters from storage */
-		parameters_update(&h, &p);
-		pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
-		pid_set_parameters(&roll_rate_controller,  p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
-		pid_set_parameters(&yaw_rate_controller,  p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f);
-	}
-
-	/* reset integrals if needed */
-	if (reset_integral) {
-		pid_reset_integral(&pitch_rate_controller);
-		pid_reset_integral(&roll_rate_controller);
-		pid_reset_integral(&yaw_rate_controller);
-	}
-
-	/* run pitch, roll and yaw controllers */
-	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
-	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
-	float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
-
-	actuators->control[0] = roll_control;
-	actuators->control[1] = pitch_control;
-	actuators->control[2] = yaw_control;
-	actuators->control[3] = rate_sp->thrust;
-
-	motor_skip_counter++;
-}
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h
deleted file mode 100644
index ca7794c59..000000000
--- a/src/modules/multirotor_att_control/multirotor_rate_control.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@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_attitude_control.h
- *
- * Definition of rate controller for multirotors.
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#ifndef MULTIROTOR_RATE_CONTROL_H_
-#define MULTIROTOR_RATE_CONTROL_H_
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-
-void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral);
-
-#endif /* MULTIROTOR_RATE_CONTROL_H_ */
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
deleted file mode 100644
index 2ca650420..000000000
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ /dev/null
@@ -1,553 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   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 multirotor_pos_control.c
- *
- * Multirotor position controller
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.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_control_mode.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_global_velocity_setpoint.h>
-#include <uORB/topics/mission_item_triplet.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/pid/pid.h>
-#include <mavlink/mavlink_log.h>
-
-#include "multirotor_pos_control_params.h"
-#include "thrust_pid.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 float scale_control(float ctl, float end, float dz);
-
-static float norm(float x, float y);
-
-static void usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\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) {
-			warnx("already running");
-			/* this is not an error */
-			exit(0);
-		}
-
-		warnx("start");
-		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("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")) {
-		warnx("stop");
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("app is running");
-
-		} else {
-			warnx("app not started");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-static float scale_control(float ctl, float end, float dz)
-{
-	if (ctl > dz) {
-		return (ctl - dz) / (end - dz);
-
-	} else if (ctl < -dz) {
-		return (ctl + dz) / (end - dz);
-
-	} else {
-		return 0.0f;
-	}
-}
-
-static float norm(float x, float y)
-{
-	return sqrtf(x * x + y * y);
-}
-
-static int multirotor_pos_control_thread_main(int argc, char *argv[])
-{
-	/* welcome user */
-	warnx("started");
-	static int mavlink_fd;
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[mpc] started");
-
-	/* structures */
-	struct vehicle_control_mode_s control_mode;
-	memset(&control_mode, 0, sizeof(control_mode));
-	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 mission_item_triplet_s triplet;
-	memset(&triplet, 0, sizeof(triplet));
-	struct vehicle_global_velocity_setpoint_s global_vel_sp;
-	memset(&global_vel_sp, 0, sizeof(global_vel_sp));
-	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 control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	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 mission_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
-
-	/* publish setpoint */
-	orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
-	orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
-	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
-
-	bool reset_mission_sp = false;
-	bool global_pos_sp_valid = false;
-	bool reset_man_sp_z = true;
-	bool reset_man_sp_xy = true;
-	bool reset_int_z = true;
-	bool reset_int_z_manual = false;
-	bool reset_int_xy = true;
-	bool was_armed = false;
-	bool reset_auto_sp_xy = true;
-	bool reset_auto_sp_z = true;
-	bool reset_takeoff_sp = true;
-
-	hrt_abstime t_prev = 0;
-	const float alt_ctl_dz = 0.2f;
-	const float pos_ctl_dz = 0.05f;
-
-	float ref_alt = 0.0f;
-	hrt_abstime ref_alt_t = 0;
-	uint64_t local_ref_timestamp = 0;
-
-	PID_t xy_pos_pids[2];
-	PID_t xy_vel_pids[2];
-	PID_t z_pos_pid;
-	thrust_pid_t z_vel_pid;
-
-	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);
-
-
-	for (int i = 0; i < 2; i++) {
-		pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
-		pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
-	}
-
-	pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
-	thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
-
-	while (!thread_should_exit) {
-
-		bool param_updated;
-		orb_check(param_sub, &param_updated);
-
-		if (param_updated) {
-			/* clear updated flag */
-			struct parameter_update_s ps;
-			orb_copy(ORB_ID(parameter_update), param_sub, &ps);
-			/* update params */
-			parameters_update(&params_h, &params);
-
-			for (int i = 0; i < 2; i++) {
-				pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
-				/* use integral_limit_out = tilt_max / 2 */
-				float i_limit;
-
-				if (params.xy_vel_i > 0.0f) {
-					i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
-
-				} else {
-					i_limit = 0.0f;	// not used
-				}
-
-				pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
-			}
-
-			pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
-			thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
-		}
-
-		bool updated;
-
-		orb_check(control_mode_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
-		}
-
-		orb_check(mission_triplet_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(mission_item_triplet), mission_triplet_sub, &triplet);
-			global_pos_sp_valid = triplet.current_valid;
-			reset_mission_sp = true;
-		}
-
-		hrt_abstime t = hrt_absolute_time();
-		float dt;
-
-		if (t_prev != 0) {
-			dt = (t - t_prev) * 0.000001f;
-
-		} else {
-			dt = 0.0f;
-		}
-
-		if (control_mode.flag_armed && !was_armed) {
-			/* reset setpoints and integrals on arming */
-			reset_man_sp_z = true;
-			reset_man_sp_xy = true;
-			reset_auto_sp_z = true;
-			reset_auto_sp_xy = true;
-			reset_takeoff_sp = true;
-			reset_int_z = true;
-			reset_int_xy = true;
-		}
-
-		was_armed = control_mode.flag_armed;
-
-		t_prev = t;
-
-		if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
-			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);
-
-			float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
-			float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
-			float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
-
-			if (control_mode.flag_control_manual_enabled) {
-				/* manual control */
-				/* check for reference point updates and correct setpoint */
-				if (local_pos.ref_timestamp != ref_alt_t) {
-					if (ref_alt_t != 0) {
-						/* home alt changed, don't follow large ground level changes in manual flight */
-						local_pos_sp.z += local_pos.ref_alt - ref_alt;
-					}
-
-					ref_alt_t = local_pos.ref_timestamp;
-					ref_alt = local_pos.ref_alt;
-					// TODO also correct XY setpoint
-				}
-
-				/* reset setpoints to current position if needed */
-				if (control_mode.flag_control_altitude_enabled) {
-					if (reset_man_sp_z) {
-						reset_man_sp_z = false;
-						local_pos_sp.z = local_pos.z;
-						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
-					}
-
-					/* move altitude setpoint with throttle stick */
-					float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
-
-					if (z_sp_ctl != 0.0f) {
-						sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
-						local_pos_sp.z += sp_move_rate[2] * dt;
-
-						if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
-							local_pos_sp.z = local_pos.z + z_sp_offs_max;
-
-						} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
-							local_pos_sp.z = local_pos.z - z_sp_offs_max;
-						}
-					}
-				}
-
-				if (control_mode.flag_control_position_enabled) {
-					if (reset_man_sp_xy) {
-						reset_man_sp_xy = false;
-						local_pos_sp.x = local_pos.x;
-						local_pos_sp.y = local_pos.y;
-						pid_reset_integral(&xy_vel_pids[0]);
-						pid_reset_integral(&xy_vel_pids[1]);
-						mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
-					}
-
-					/* move position setpoint with roll/pitch stick */
-					float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
-					float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
-
-					if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
-						/* calculate direction and increment of control in NED frame */
-						float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
-						float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
-						sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
-						sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
-						local_pos_sp.x += sp_move_rate[0] * dt;
-						local_pos_sp.y += sp_move_rate[1] * dt;
-						/* limit maximum setpoint from position offset and preserve direction
-						 * fail safe, should not happen in normal operation */
-						float pos_vec_x = local_pos_sp.x - local_pos.x;
-						float pos_vec_y = local_pos_sp.y - local_pos.y;
-						float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
-
-						if (pos_vec_norm > 1.0f) {
-							local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
-							local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
-						}
-					}
-				}
-
-				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
-				local_pos_sp.yaw = att_sp.yaw_body;
-
-				/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
-				reset_auto_sp_xy = !control_mode.flag_control_position_enabled;
-				reset_auto_sp_z = !control_mode.flag_control_altitude_enabled;
-				reset_takeoff_sp = true;
-
-				/* force reprojection of global setpoint after manual mode */
-				reset_mission_sp = true;
-			}
-			/* AUTO not implemented */
-
-			/* publish local position setpoint */
-			orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
-
-			/* run position & altitude controllers, calculate velocity setpoint */
-			if (control_mode.flag_control_altitude_enabled) {
-				global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
-
-			} else {
-				reset_man_sp_z = true;
-				global_vel_sp.vz = 0.0f;
-			}
-
-			if (control_mode.flag_control_position_enabled) {
-				/* calculate velocity set point in NED frame */
-				global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
-				global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
-
-				/* limit horizontal speed */
-				float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
-
-				if (xy_vel_sp_norm > 1.0f) {
-					global_vel_sp.vx /= xy_vel_sp_norm;
-					global_vel_sp.vy /= xy_vel_sp_norm;
-				}
-
-			} else {
-				reset_man_sp_xy = true;
-				global_vel_sp.vx = 0.0f;
-				global_vel_sp.vy = 0.0f;
-			}
-
-			/* publish new velocity setpoint */
-			orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
-			// TODO subscribe to velocity setpoint if altitude/position control disabled
-
-			if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
-				/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
-				float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
-
-				if (control_mode.flag_control_climb_rate_enabled) {
-					if (reset_int_z) {
-						reset_int_z = false;
-						float i = params.thr_min;
-
-						if (reset_int_z_manual) {
-							i = manual.throttle;
-
-							if (i < params.thr_min) {
-								i = params.thr_min;
-
-							} else if (i > params.thr_max) {
-								i = params.thr_max;
-							}
-						}
-
-						thrust_pid_set_integral(&z_vel_pid, -i);
-						mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
-					}
-
-					thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
-					att_sp.thrust = -thrust_sp[2];
-
-				} else {
-					/* reset thrust integral when altitude control enabled */
-					reset_int_z = true;
-				}
-
-				if (control_mode.flag_control_velocity_enabled) {
-					/* calculate thrust set point in NED frame */
-					if (reset_int_xy) {
-						reset_int_xy = false;
-						pid_reset_integral(&xy_vel_pids[0]);
-						pid_reset_integral(&xy_vel_pids[1]);
-						mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
-					}
-
-					thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
-					thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
-
-					/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
-					/* limit horizontal part of thrust */
-					float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
-					/* assuming that vertical component of thrust is g,
-					 * horizontal component = g * tan(alpha) */
-					float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
-
-					if (tilt > params.tilt_max) {
-						tilt = params.tilt_max;
-					}
-
-					/* convert direction to body frame */
-					thrust_xy_dir -= att.yaw;
-					/* calculate roll and pitch */
-					att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
-					att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
-
-				} else {
-					reset_int_xy = true;
-				}
-
-				att_sp.timestamp = hrt_absolute_time();
-
-				/* publish new attitude setpoint */
-				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-			}
-
-		} else {
-			/* position controller disabled, reset setpoints */
-			reset_man_sp_z = true;
-			reset_man_sp_xy = true;
-			reset_int_z = true;
-			reset_int_xy = true;
-			reset_mission_sp = true;
-			reset_auto_sp_xy = true;
-			reset_auto_sp_z = true;
-		}
-
-		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
-		reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
-
-		/* run at approximately 50 Hz */
-		usleep(20000);
-	}
-
-	warnx("stopped");
-	mavlink_log_info(mavlink_fd, "[mpc] stopped");
-
-	thread_running = false;
-
-	fflush(stdout);
-	return 0;
-}
-