initial wip version of launchdetector

makefiles/config_px4fmu-v1_default.mk

@@ -116,6 +116,7 @@ MODULES		+= lib/ecl
 MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
 MODULES		+= lib/conversion
+MODULES		+= lib/launchdetection
 
 #
 # Demo apps

makefiles/config_px4fmu-v2_default.mk

@@ -113,6 +113,7 @@ MODULES		+= lib/ecl
 MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
 MODULES		+= lib/conversion
+MODULES		+= lib/launchdetection
 
 #
 # Demo apps

src/lib/launchdetection/CatapultLaunchMethod.cpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 documentation4 and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL 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 CatapultLaunchMethod.cpp
+ * Catpult Launch detection
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "CatapultLaunchMethod.h"
+
+CatapultLaunchMethod::CatapultLaunchMethod() :
+	last_timestamp(0),
+	integrator(0.0f),
+	launchDetected(false),
+	threshold_accel(NULL, "LAUN_CAT_A", false),
+	threshold_time(NULL, "LAUN_CAT_T", false)
+{
+
+}
+
+CatapultLaunchMethod::~CatapultLaunchMethod() {
+
+}
+
+void CatapultLaunchMethod::update(float accel_x)
+{
+	float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
+	last_timestamp = hrt_absolute_time();
+
+	if (accel_x > threshold_accel.get()) {
+		integrator += accel_x * dt;
+		if (integrator > threshold_accel.get() * threshold_time.get()) {
+			launchDetected = true;
+		}
+
+	} else {
+		/* reset integrator */
+		integrator = 0.0f;
+		launchDetected = false;
+	}
+
+}
+
+bool CatapultLaunchMethod::getLaunchDetected()
+{
+	return launchDetected;
+}
+
+void CatapultLaunchMethod::updateParams()
+{
+	threshold_accel.update();
+	threshold_time.update();
+}

src/lib/launchdetection/CatapultLaunchMethod.h

@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 documentation4 and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL 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 CatapultLaunchMethod.h
+ * Catpult Launch detection
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef CATAPULTLAUNCHMETHOD_H_
+#define CATAPULTLAUNCHMETHOD_H_
+
+#include "LaunchMethod.h"
+
+#include <drivers/drv_hrt.h>
+#include <controllib/block/BlockParam.hpp>
+
+class CatapultLaunchMethod : public LaunchMethod
+{
+public:
+	CatapultLaunchMethod();
+	~CatapultLaunchMethod();
+
+	void update(float accel_x);
+	bool getLaunchDetected();
+	void updateParams();
+
+private:
+	hrt_abstime last_timestamp;
+//	float threshold_accel_raw;
+//	float threshold_time;
+	float integrator;
+	bool launchDetected;
+
+	control::BlockParamFloat threshold_accel;
+	control::BlockParamFloat threshold_time;
+
+};
+
+#endif /* CATAPULTLAUNCHMETHOD_H_ */

src/lib/launchdetection/LaunchDetector.cpp

@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 launchDetection.cpp
+ * Auto Detection for different launch methods (e.g. catapult)
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "LaunchDetector.h"
+#include "CatapultLaunchMethod.h"
+#include <systemlib/err.h>
+
+LaunchDetector::LaunchDetector() :
+	launchdetection_on(NULL, "LAUN_ALL_ON", false)
+{
+	/* init all detectors */
+	launchMethods[0] = new CatapultLaunchMethod();
+
+
+	/* update all parameters of all detectors */
+	updateParams();
+}
+
+LaunchDetector::~LaunchDetector()
+{
+
+}
+
+void LaunchDetector::update(float accel_x)
+{
+	if (launchdetection_on.get() == 1) {
+		for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+			launchMethods[i]->update(accel_x);
+		}
+	}
+}
+
+bool LaunchDetector::getLaunchDetected()
+{
+	if (launchdetection_on.get() == 1) {
+		for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+			if(launchMethods[i]->getLaunchDetected()) {
+				return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+void LaunchDetector::updateParams() {
+
+	warnx(" LaunchDetector::updateParams()");
+	//launchdetection_on.update();
+
+	for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+		//launchMethods[i]->updateParams();
+		warnx("updating component %d", i);
+	}
+
+	warnx(" LaunchDetector::updateParams() ended");
+}

src/lib/launchdetection/LaunchDetector.h

@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 documentation4 and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL 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 LaunchDetector.h
+ * Auto Detection for different launch methods (e.g. catapult)
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef LAUNCHDETECTOR_H
+#define LAUNCHDETECTOR_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "LaunchMethod.h"
+
+#include <controllib/block/BlockParam.hpp>
+
+class __EXPORT LaunchDetector
+{
+public:
+	LaunchDetector();
+	~LaunchDetector();
+
+	void update(float accel_x);
+	bool getLaunchDetected();
+	void updateParams();
+	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
+
+//	virtual bool getLaunchDetected();
+protected:
+private:
+	LaunchMethod* launchMethods[1];
+	control::BlockParamInt launchdetection_on;
+
+
+};
+
+
+#endif // LAUNCHDETECTOR_H

src/lib/launchdetection/LaunchMethod.h

@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 documentation4 and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL 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 LaunchMethod.h
+ * Base class for different launch methods
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef LAUNCHMETHOD_H_
+#define LAUNCHMETHOD_H_
+
+class LaunchMethod
+{
+public:
+	virtual void update(float accel_x) = 0;
+	virtual bool getLaunchDetected() = 0;
+	virtual void updateParams() = 0;
+protected:
+private:
+};
+
+#endif /* LAUNCHMETHOD_H_ */

src/lib/launchdetection/launchdetection_params.c

@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 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 fw_pos_control_l1_params.c
+ *
+ * Parameters defined by the L1 position control task
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Launch detection parameters, accessible via MAVLink
+ *
+ */
+
+/* Catapult Launch detection */
+
+// @DisplayName		Switch to enable launchdetection
+// @Description		if set to 1 launchdetection is enabled
+// @Range		0 or 1
+PARAM_DEFINE_INT32(LAUN_ALL_ON, 0);
+
+// @DisplayName		Catapult Accelerometer Threshold
+// @Description		LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
+// @Range		> 0
+PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
+
+// @DisplayName		Catapult Time Threshold
+// @Description		LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
+// @Range		> 0, in seconds
+PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);

src/lib/launchdetection/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2013 Estimation and Control Library (ECL). 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.
+#
+############################################################################
+
+#
+# Launchdetection Library
+#
+
+SRCS		 = 	LaunchDetector.cpp \
+			CatapultLaunchMethod.cpp \
+			launchdetection_params.c

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -86,6 +86,7 @@
 
 #include <ecl/l1/ecl_l1_pos_controller.h>
 #include <external_lgpl/tecs/tecs.h>
+#include <launchdetection/LaunchDetector.h>
 
 /**
  * L1 control app start / stop handling function
@@ -164,6 +165,9 @@ private:
 	/* heading hold */
 	float target_bearing;
 
+	/* Launch detection */
+	LaunchDetector launchDetector;
+
 	/* throttle and airspeed states */
 	float _airspeed_error;				///< airspeed error to setpoint in m/s
 	bool _airspeed_valid;				///< flag if a valid airspeed estimate exists
@@ -338,7 +342,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_airspeed_valid(false),
 	_groundspeed_undershoot(0.0f),
 	_global_pos_valid(false),
-	land_noreturn(false)
+	land_noreturn(false),
+	launchDetector()
 {
 	_nav_capabilities.turn_distance = 0.0f;
 
@@ -464,6 +469,8 @@ FixedwingPositionControl::parameters_update()
 		return 1;
 	}
 
+	launchDetector.updateParams();
+
 	return OK;
 }
 
@@ -818,30 +825,50 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 			} else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
 
+				/* Perform launch detection */
+				bool do_fly_takeoff = false;
+				warnx("Launch detection running");
+				if (launchDetector.launchDetectionEnabled()) {
+					launchDetector.update(_accel.x);
+					if (launchDetector.getLaunchDetected()) {
+						do_fly_takeoff = true;
+						warnx("Launch detected. Taking off!");
+					}
+				} else	{
+					/* no takeoff detection --> fly */
+					do_fly_takeoff = true;
+				}
+
+
 				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
 
-				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
+				if (do_fly_takeoff) {
-				if (altitude_error > 10.0f) {
 
-					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
+					/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
+					if (altitude_error > 10.0f) {
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)),
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
-					/* limit roll motion to ensure enough lift */
+						/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
-					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
+						_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
+									    _airspeed.indicated_airspeed_m_s, eas2tas,
+									    true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)),
+									    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+									    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
+						/* limit roll motion to ensure enough lift */
+						_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
+
+					} else {
+
+						_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+									    _airspeed.indicated_airspeed_m_s, eas2tas,
+									    false, math::radians(_parameters.pitch_limit_min),
+									    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+									    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+					}
 				} else {
-
+					throttle_max = 0.0f;
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, math::radians(_parameters.pitch_limit_min),
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 				}
 			}