Merge branch 'master' of github.com:PX4/Firmware into paul_estimator

16 files changed, 253 insertions, 65 deletions

diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py
new file mode 100644
index 000000000..ceef9f9be
--- /dev/null
+++ b/Tools/px_romfs_pruner.py
@@ -0,0 +1,83 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+#   Author: 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.
+#
+############################################################################
+
+
+"""
+px_romfs_pruner.py:
+Delete all comments and newlines before ROMFS is converted to an image
+"""
+
+from __future__ import print_function
+import argparse
+import os
+
+def main():
+    
+        # Parse commandline arguments
+        parser = argparse.ArgumentParser(description="ROMFS pruner.")
+        parser.add_argument('--folder', action="store", help="ROMFS scratch folder.")
+        args = parser.parse_args()
+    
+        print("Pruning ROMFS files.")
+        
+        # go through 
+        for (root, dirs, files) in os.walk(args.folder):
+                for file in files:
+                        # only prune text files
+                        if ".zip" in file or ".bin" in file:
+                                continue
+                        
+                        file_path = os.path.join(root, file)                        
+                        
+                        # read file line by line
+                        pruned_content = ""
+                        with open(file_path, "r") as f:
+                                for line in  f:
+                                        
+                                        # handle mixer files differently than startup files
+                                        if file_path.endswith(".mix"):
+                                                if line.startswith(("Z:", "M:", "R: ", "O:", "S:")):
+                                                        pruned_content += line
+                                        else:
+                                                if not line.isspace() and not line.strip().startswith("#"):
+                                                        pruned_content += line
+                        
+                        # overwrite old scratch file
+                        with open(file_path, "w") as f:
+                                f.write(pruned_content)
+                        
+    
+if __name__ == '__main__':
+        main()
\ No newline at end of file
diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk
index cb20d9cd1..1b646d9e0 100644
--- a/makefiles/firmware.mk
+++ b/makefiles/firmware.mk
@@ -355,6 +355,9 @@ ROMFS_OBJ		 = $(ROMFS_CSRC:.c=.o)
 LIBS			+= $(ROMFS_OBJ)
 LINK_DEPS		+= $(ROMFS_OBJ)
 
+# Remove all comments from startup and mixer files
+ROMFS_PRUNER	 = $(PX4_BASE)/Tools/px_romfs_pruner.py
+
 # Turn the ROMFS image into an object file
 $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS)
 	$(call BIN_TO_OBJ,$<,$@,romfs_img)
@@ -372,6 +375,7 @@ ifneq ($(ROMFS_EXTRA_FILES),)
 	$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras
 	$(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras
 endif
+	$(Q) $(PYTHON) -u $(ROMFS_PRUNER) --folder $(ROMFS_SCRATCH)
 
 EXTRA_CLEANS		+= $(ROMGS_OBJ) $(ROMFS_IMG)
 
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index 1dc96b3c3..20edc68aa 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -417,7 +417,7 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=6000
+CONFIG_IDLETHREAD_STACKSIZE=4096
 CONFIG_USERMAIN_STACKSIZE=4096
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
@@ -720,11 +720,11 @@ CONFIG_SCHED_WORKQUEUE=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_WORKPRIORITY=192
 CONFIG_SCHED_WORKPERIOD=5000
-CONFIG_SCHED_WORKSTACKSIZE=4000
+CONFIG_SCHED_WORKSTACKSIZE=2048
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKPERIOD=50000
-CONFIG_SCHED_LPWORKSTACKSIZE=4000
+CONFIG_SCHED_LPWORKSTACKSIZE=2048
 # CONFIG_LIB_KBDCODEC is not set
 # CONFIG_LIB_SLCDCODEC is not set
 
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
index d5c759b17..12f80c4a3 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.cpp
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -41,12 +41,16 @@
 #include "CatapultLaunchMethod.h"
 #include <systemlib/err.h>
 
-CatapultLaunchMethod::CatapultLaunchMethod() :
-	last_timestamp(0),
+namespace launchdetection
+{
+
+CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
+	SuperBlock(parent, "CAT"),
+	last_timestamp(hrt_absolute_time()),
 	integrator(0.0f),
 	launchDetected(false),
-	threshold_accel(NULL, "LAUN_CAT_A", false),
-	threshold_time(NULL, "LAUN_CAT_T", false)
+	threshold_accel(this, "A"),
+	threshold_time(this, "T")
 {
 
 }
@@ -83,8 +87,11 @@ bool CatapultLaunchMethod::getLaunchDetected()
 	return launchDetected;
 }
 
-void CatapultLaunchMethod::updateParams()
+
+void CatapultLaunchMethod::reset()
 {
-	threshold_accel.update();
-	threshold_time.update();
+	integrator = 0.0f;
+	launchDetected = false;
+}
+
 }
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
index e943f11e9..55c46ff3f 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.h
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -44,22 +44,24 @@
 #include "LaunchMethod.h"
 
 #include <drivers/drv_hrt.h>
+#include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
-class CatapultLaunchMethod : public LaunchMethod
+namespace launchdetection
+{
+
+class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock
 {
 public:
-	CatapultLaunchMethod();
+	CatapultLaunchMethod(SuperBlock *parent);
 	~CatapultLaunchMethod();
 
 	void update(float accel_x);
 	bool getLaunchDetected();
-	void updateParams();
+	void reset();
 
 private:
 	hrt_abstime last_timestamp;
-//	float threshold_accel_raw;
-//	float threshold_time;
 	float integrator;
 	bool launchDetected;
 
@@ -69,3 +71,5 @@ private:
 };
 
 #endif /* CATAPULTLAUNCHMETHOD_H_ */
+
+}
diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
index df9f2fe95..bf539701b 100644
--- a/src/lib/launchdetection/LaunchDetector.cpp
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -42,12 +42,16 @@
 #include "CatapultLaunchMethod.h"
 #include <systemlib/err.h>
 
+namespace launchdetection
+{
+
 LaunchDetector::LaunchDetector() :
-	launchdetection_on(NULL, "LAUN_ALL_ON", false),
-	throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
+	SuperBlock(NULL, "LAUN"),
+	launchdetection_on(this, "ALL_ON"),
+	throttlePreTakeoff(this, "THR_PRE")
 {
 	/* init all detectors */
-	launchMethods[0] = new CatapultLaunchMethod();
+	launchMethods[0] = new CatapultLaunchMethod(this);
 
 
 	/* update all parameters of all detectors */
@@ -59,6 +63,12 @@ LaunchDetector::~LaunchDetector()
 
 }
 
+void LaunchDetector::reset()
+{
+	/* Reset all detectors */
+	launchMethods[0]->reset();
+}
+
 void LaunchDetector::update(float accel_x)
 {
 	if (launchdetection_on.get() == 1) {
@@ -81,12 +91,4 @@ bool LaunchDetector::getLaunchDetected()
 	return false;
 }
 
-void LaunchDetector::updateParams() {
-
-	launchdetection_on.update();
-	throttlePreTakeoff.update();
-
-	for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
-		launchMethods[i]->updateParams();
-	}
 }
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
index 7c2ff826c..8066ebab3 100644
--- a/src/lib/launchdetection/LaunchDetector.h
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -45,18 +45,21 @@
 #include <stdint.h>
 
 #include "LaunchMethod.h"
-
+#include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
-class __EXPORT LaunchDetector
+namespace launchdetection
+{
+
+class __EXPORT LaunchDetector : public control::SuperBlock
 {
 public:
 	LaunchDetector();
 	~LaunchDetector();
+	void reset();
 
 	void update(float accel_x);
 	bool getLaunchDetected();
-	void updateParams();
 	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
 
 	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
@@ -71,5 +74,6 @@ private:
 
 };
 
+}
 
 #endif // LAUNCHDETECTOR_H
diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h
index bfb5f8cb4..01fa7050e 100644
--- a/src/lib/launchdetection/LaunchMethod.h
+++ b/src/lib/launchdetection/LaunchMethod.h
@@ -41,14 +41,20 @@
 #ifndef LAUNCHMETHOD_H_
 #define LAUNCHMETHOD_H_
 
+namespace launchdetection
+{
+
 class LaunchMethod
 {
 public:
 	virtual void update(float accel_x) = 0;
 	virtual bool getLaunchDetected() = 0;
-	virtual void updateParams() = 0;
+	virtual void reset() = 0;
+
 protected:
 private:
 };
 
+}
+
 #endif /* LAUNCHMETHOD_H_ */
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 3881bdf55..10a6cd2c5 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -473,6 +473,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						dt = 0.005f;
 						parameters_update(&ekf_param_handles, &ekf_params);
 
+						/* update mag declination rotation matrix */
+						R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
+
 						x_aposteriori_k[0] = z_k[0];
 						x_aposteriori_k[1] = z_k[1];
 						x_aposteriori_k[2] = z_k[2];
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 17b1028f9..5ade835ff 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -166,6 +166,15 @@ private:
 		float airspeed_min;
 		float airspeed_trim;
 		float airspeed_max;
+
+		float trim_roll;
+		float trim_pitch;
+		float trim_yaw;
+		float rollsp_offset_deg;			/**< Roll Setpoint Offset in deg */
+		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
+		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
+		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -197,6 +206,12 @@ private:
 		param_t airspeed_min;
 		param_t airspeed_trim;
 		param_t airspeed_max;
+
+		param_t trim_roll;
+		param_t trim_pitch;
+		param_t trim_yaw;
+		param_t rollsp_offset_deg;
+		param_t pitchsp_offset_deg;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -335,6 +350,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
 	_parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN");
 
+	_parameter_handles.trim_roll = param_find("TRIM_ROLL");
+	_parameter_handles.trim_pitch = param_find("TRIM_PITCH");
+	_parameter_handles.trim_yaw = param_find("TRIM_YAW");
+	_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
+	_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -395,6 +416,15 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
 	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
 
+	param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
+	param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
+	param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
+	param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
+	param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
+	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
+	_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
+
+
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
 	_pitch_ctrl.set_k_p(_parameters.p_p);
@@ -648,13 +678,13 @@ FixedwingAttitudeControl::task_main()
 				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
 				//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
 
-				float roll_sp = 0.0f;
-				float pitch_sp = 0.0f;
+				float roll_sp = _parameters.rollsp_offset_rad;
+				float pitch_sp = _parameters.pitchsp_offset_rad;
 				float throttle_sp = 0.0f;
 
 				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
-					roll_sp = _att_sp.roll_body;
-					pitch_sp = _att_sp.pitch_body;
+					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
+					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
 					throttle_sp = _att_sp.thrust;
 
 					/* reset integrals where needed */
@@ -670,9 +700,13 @@ FixedwingAttitudeControl::task_main()
 					 * With this mapping the stick angle is a 1:1 representation of
 					 * the commanded attitude. If more than 45 degrees are desired,
 					 * a scaling parameter can be applied to the remote.
+					 *
+					 * The trim gets subtracted here from the manual setpoint to get
+					 * the intended attitude setpoint. Later, after the rate control step the
+					 * trim is added again to the control signal.
 					 */
-					roll_sp = _manual.roll * 0.75f;
-					pitch_sp = _manual.pitch * 0.75f;
+					roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad;
+					pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad;
 					throttle_sp = _manual.throttle;
 					_actuators.control[4] = _manual.flaps;
 
@@ -685,7 +719,7 @@ FixedwingAttitudeControl::task_main()
 					att_sp.timestamp = hrt_absolute_time();
 					att_sp.roll_body = roll_sp;
 					att_sp.pitch_body = pitch_sp;
-					att_sp.yaw_body = 0.0f;
+					att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
 					att_sp.thrust = throttle_sp;
 
 					/* lazily publish the setpoint only once available */
@@ -719,12 +753,12 @@ FixedwingAttitudeControl::task_main()
 							speed_body_u, speed_body_v, speed_body_w,
 							_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
 
-					/* Run attitude RATE controllers which need the desired attitudes from above */
+					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
 					float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
 							_att.rollspeed, _att.yawspeed,
 							_yaw_ctrl.get_desired_rate(),
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f;
+					_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
 					if (!isfinite(roll_u)) {
 						warnx("roll_u %.4f", roll_u);
 					}
@@ -733,7 +767,7 @@ FixedwingAttitudeControl::task_main()
 							_att.pitchspeed, _att.yawspeed,
 							_yaw_ctrl.get_desired_rate(),
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f;
+					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
 					if (!isfinite(pitch_u)) {
 						warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
 								pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body);
@@ -743,7 +777,7 @@ FixedwingAttitudeControl::task_main()
 							_att.pitchspeed, _att.yawspeed,
 							_pitch_ctrl.get_desired_rate(),
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f;
+					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
 					if (!isfinite(yaw_u)) {
 						warnx("yaw_u %.4f", yaw_u);
 					}
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index 1c615094c..c80a44f2a 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -176,3 +176,13 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
 // @Description		If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
 // @Range		0.0 to 30
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
+
+// @DisplayName		Roll Setpoint Offset
+// @Description		An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe
+// @Range		-90.0 to 90.0
+PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
+
+// @DisplayName		Pitch Setpoint Offset
+// @Description		An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
+// @Range		-90.0 to 90.0
+PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index ed6d8792c..7f13df785 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -176,6 +176,8 @@ private:
 	bool launch_detected;
 	bool usePreTakeoffThrust;
 
+	bool last_manual;				///< true if the last iteration was in manual mode (used to determine when a reset is needed)
+
 	/* Landingslope object */
 	Landingslope landingslope;
 
@@ -184,7 +186,7 @@ private:
 	float target_bearing;
 
 	/* Launch detection */
-	LaunchDetector launchDetector;
+	launchdetection::LaunchDetector launchDetector;
 
 	/* throttle and airspeed states */
 	float _airspeed_error;				///< airspeed error to setpoint in m/s
@@ -344,6 +346,16 @@ private:
 	 * Main sensor collection task.
 	 */
 	void		task_main() __attribute__((noreturn));
+
+	/*
+	 * Reset takeoff state
+	 */
+	void reset_takeoff_state();
+
+	/*
+	 * Reset landing state
+	 */
+	void reset_landing_state();
 };
 
 namespace l1_control
@@ -389,6 +401,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_motor_lim(false),
 	land_onslope(false),
 	launch_detected(false),
+	last_manual(false),
 	usePreTakeoffThrust(false),
 	flare_curve_alt_last(0.0f),
 	launchDetector(),
@@ -976,6 +989,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				} else	{
 					/* no takeoff detection --> fly */
 					launch_detected = true;
+					warnx("launchdetection off");
 				}
 			}
 
@@ -1022,19 +1036,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		// mission is active
 		_loiter_hold = false;
 
-		/* reset land state */
+		/* reset landing state */
 		if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
-			land_noreturn_horizontal = false;
-			land_noreturn_vertical = false;
-			land_stayonground = false;
-			land_motor_lim = false;
-			land_onslope = false;
+			reset_landing_state();
 		}
 
 		/* reset takeoff/launch state */
 		if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
-			launch_detected = false;
-			usePreTakeoffThrust = false;
+			reset_takeoff_state();
 		}
 
 		if (was_circle_mode && !_l1_control.circle_mode()) {
@@ -1131,6 +1140,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		/* no flight mode applies, do not publish an attitude setpoint */
 		setpoint = false;
+
+		/* reset landing and takeoff state */
+		if (!last_manual) {
+			reset_landing_state();
+			reset_takeoff_state();
+		}
 	}
 
 	if (usePreTakeoffThrust) {
@@ -1141,6 +1156,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	}
 	_att_sp.pitch_body = _tecs.get_pitch_demand();
 
+	if (_control_mode.flag_control_position_enabled) {
+		last_manual = false;
+	} else {
+		last_manual = true;
+	}
+
 
 	return setpoint;
 }
@@ -1291,6 +1312,22 @@ FixedwingPositionControl::task_main()
 	_exit(0);
 }
 
+void FixedwingPositionControl::reset_takeoff_state()
+{
+	launch_detected = false;
+	usePreTakeoffThrust = false;
+	launchDetector.reset();
+}
+
+void FixedwingPositionControl::reset_landing_state()
+{
+	land_noreturn_horizontal = false;
+	land_noreturn_vertical = false;
+	land_stayonground = false;
+	land_motor_lim = false;
+	land_onslope = false;
+}
+
 int
 FixedwingPositionControl::start()
 {
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 9bbaf167a..f452a85f7 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -55,11 +55,13 @@
 #endif
 static const int ERROR = -1;
 
-Geofence::Geofence() : _fence_pub(-1),
+Geofence::Geofence() :
+		SuperBlock(NULL, "GF"),
+		_fence_pub(-1),
 		_altitude_min(0),
 		_altitude_max(0),
 		_verticesCount(0),
-		param_geofence_on(NULL, "GF_ON", false)
+		param_geofence_on(this, "ON")
 {
 	/* Load initial params */
 	updateParams();
@@ -292,8 +294,3 @@ int Geofence::clearDm()
 {
 	dm_clear(DM_KEY_FENCE_POINTS);
 }
-
-void Geofence::updateParams()
-{
-	param_geofence_on.update();
-}
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index 5b56ebc7a..9628b7271 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -41,11 +41,13 @@
 #define GEOFENCE_H_
 
 #include <uORB/topics/fence.h>
+#include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
 #define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt"
 
-class Geofence {
+class Geofence : public control::SuperBlock
+{
 private:
 	orb_advert_t	_fence_pub;			/**< publish fence topic */
 
@@ -85,8 +87,6 @@ public:
 	int loadFromFile(const char *filename);
 
 	bool isEmpty() {return _verticesCount == 0;}
-
-	void updateParams();
 };
 
 
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
index 13328edb4..7cd076948 100644
--- a/src/modules/position_estimator_inav/inertial_filter.c
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -15,10 +15,7 @@ void inertial_filter_predict(float dt, float x[3])
 
 void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
 {
-	float ewdt = w * dt;
-	if (ewdt > 1.0f)
-		ewdt = 1.0f;	// prevent over-correcting
-	ewdt *= e;
+	float ewdt = e * w * dt;
 	x[i] += ewdt;
 
 	if (i == 0) {
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 37f507086..368fa6ee2 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -623,7 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		}
 
 		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
-		dt = fmaxf(fminf(0.02, dt), 0.005);
+		dt = fmaxf(fminf(0.02, dt), 0.002);		// constrain dt from 2 to 20 ms
 		t_prev = t;
 
 		/* use GPS if it's valid and reference position initialized */