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

Tools/fetch_log.py

@@ -0,0 +1,133 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Log fetcher
+#
+# Print list of logs:
+#     python fetch_log.py
+#
+# Fetch log:
+#     python fetch_log.py sess001/log001.bin
+#
+
+import serial, time, sys, os
+
+def wait_for_string(ser, s, timeout=1.0, debug=False):
+    t0 = time.time()
+    buf = []
+    res = []
+    n = 0
+    while (True):
+        c = ser.read()
+        if debug:
+            sys.stderr.write(c)
+        buf.append(c)
+        if len(buf) > len(s):
+            res.append(buf.pop(0))
+            n += 1
+            if n % 10000 == 0:
+                sys.stderr.write(str(n) + "\n")
+        if "".join(buf) == s:
+            break
+        if timeout > 0.0 and time.time() - t0 > timeout:
+            raise Exception("Timeout while waiting for: " + s)
+    return "".join(res)
+
+def exec_cmd(ser, cmd, timeout):
+    ser.write(cmd + "\n")
+    ser.flush()
+    wait_for_string(ser, cmd + "\r\n", timeout)
+    return wait_for_string(ser, "nsh> \x1b[K", timeout)
+
+def ls_dir(ser, dir, timeout=1.0):
+    res = []
+    for line in exec_cmd(ser, "ls -l " + dir, timeout).splitlines()[1:]:
+        res.append((line[20:], int(line[11:19].strip()), line[1] == "d"))
+    return res
+
+def list_logs(ser):
+    logs_dir = "/fs/microsd/log"
+    res = []
+    for d in ls_dir(ser, logs_dir):
+        if d[2]:
+            sess_dir = d[0][:-1]
+            for f in ls_dir(ser, logs_dir + "/" + sess_dir):
+                log_file = f[0]
+                log_size = f[1]
+                res.append(sess_dir + "/" + log_file + "\t" + str(log_size))
+    return "\n".join(res)
+
+def fetch_log(ser, fn, timeout):
+    cmd = "dumpfile " + fn
+    ser.write(cmd + "\n")
+    ser.flush()
+    wait_for_string(ser, cmd + "\r\n", timeout, True)
+    res = wait_for_string(ser, "\n", timeout, True)
+    data = []
+    if res.startswith("OK"):
+        size = int(res.split()[1])
+        n = 0
+        print "Reading data:"
+        while (n < size):
+            buf = ser.read(min(size - n, 8192))
+            data.append(buf)
+            n += len(buf)
+            sys.stdout.write(".")
+            sys.stdout.flush()
+        print
+    else:
+        raise Exception("Error reading log")
+    wait_for_string(ser, "nsh> \x1b[K", timeout)
+    return "".join(data)
+
+def main():
+    dev = "/dev/tty.usbmodem1"
+    ser = serial.Serial(dev, "115200", timeout=0.2)
+    if len(sys.argv) < 2:
+        print list_logs(ser)
+    else:
+        log_file = sys.argv[1]
+        data = fetch_log(ser, "/fs/microsd/log/" + log_file, 1.0)
+        try:
+            os.mkdir(log_file.split("/")[0])
+        except:
+            pass
+        fout = open(log_file, "wb")
+        fout.write(data)
+        fout.close()
+    ser.close()
+
+if __name__ == "__main__":
+    main()

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()

makefiles/config_px4fmu-v1_default.mk

@@ -56,6 +56,7 @@ MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/hw_ver
+MODULES		+= systemcmds/dumpfile
 
 #
 # General system control

makefiles/config_px4fmu-v2_default.mk

@@ -63,6 +63,7 @@ MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/hw_ver
+MODULES		+= systemcmds/dumpfile
 
 #
 # General system control

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)
 

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
 

src/lib/launchdetection/CatapultLaunchMethod.cpp

@@ -41,12 +41,16 @@
 #include "CatapultLaunchMethod.h"
 #include <systemlib/err.h>
 
-CatapultLaunchMethod::CatapultLaunchMethod() :
+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_accel(this, "A"),
-	threshold_time(NULL, "LAUN_CAT_T", false)
+	threshold_time(this, "T")
 {
 
 }
@@ -83,14 +87,11 @@ bool CatapultLaunchMethod::getLaunchDetected()
 	return launchDetected;
 }
 
-void CatapultLaunchMethod::updateParams()
-{
-	threshold_accel.update();
-	threshold_time.update();
-}
 
 void CatapultLaunchMethod::reset()
 {
 	integrator = 0.0f;
 	launchDetected = false;
 }
+
+}

src/lib/launchdetection/CatapultLaunchMethod.h

@@ -44,17 +44,20 @@
 #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:
@@ -68,3 +71,5 @@ private:
 };
 
 #endif /* CATAPULTLAUNCHMETHOD_H_ */
+
+}

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),
+	SuperBlock(NULL, "LAUN"),
-	throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
+	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 */
@@ -87,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();
-	}
 }

src/lib/launchdetection/LaunchDetector.h

@@ -45,10 +45,13 @@
 #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();
@@ -57,7 +60,6 @@ public:
 
 	void update(float accel_x);
 	bool getLaunchDetected();
-	void updateParams();
 	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
 
 	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
@@ -72,5 +74,6 @@ private:
 
 };
 
+}
 
 #endif // LAUNCHDETECTOR_H

src/lib/launchdetection/LaunchMethod.h

@@ -41,15 +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_ */

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -477,6 +477,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];

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 roll_sp = _parameters.rollsp_offset_rad;
-				float pitch_sp = 0.0f;
+				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;
+					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
-					pitch_sp = _att_sp.pitch_body;
+					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;
+					roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad;
-					pitch_sp = _manual.pitch * 0.75f;
+					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);
 					}

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);

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -186,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
@@ -350,12 +350,12 @@ private:
 	/*
 	 * Reset takeoff state
 	 */
-	int reset_takeoff_state();
+	void reset_takeoff_state();
 
 	/*
 	 * Reset landing state
 	 */
-	int reset_landing_state();
+	void reset_landing_state();
 };
 
 namespace l1_control
@@ -989,6 +989,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				} else	{
 					/* no takeoff detection --> fly */
 					launch_detected = true;
+					warnx("launchdetection off");
 				}
 			}
 
@@ -1311,14 +1312,14 @@ FixedwingPositionControl::task_main()
 	_exit(0);
 }
 
-int FixedwingPositionControl::reset_takeoff_state()
+void FixedwingPositionControl::reset_takeoff_state()
 {
 	launch_detected = false;
 	usePreTakeoffThrust = false;
 	launchDetector.reset();
 }
 
-int FixedwingPositionControl::reset_landing_state()
+void FixedwingPositionControl::reset_landing_state()
 {
 	land_noreturn_horizontal = false;
 	land_noreturn_vertical = false;

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();
-}

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();
 };
 
 

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;
+	float ewdt = e * w * dt;
-	if (ewdt > 1.0f)
-		ewdt = 1.0f;	// prevent over-correcting
-	ewdt *= e;
 	x[i] += ewdt;
 
 	if (i == 0) {

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 */

src/modules/sdlog2/logbuffer.c

@@ -74,8 +74,9 @@ bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
 	// bytes available to write
 	int available = lb->read_ptr - lb->write_ptr - 1;
 
-	if (available < 0)
+	if (available < 0) {
 		available += lb->size;
+	}
 
 	if (size > available) {
 		// buffer overflow

src/modules/sdlog2/sdlog2_messages.h

@@ -267,13 +267,13 @@ struct log_DIST_s {
 /* --- TELE - TELEMETRY STATUS --- */
 #define LOG_TELE_MSG 22
 struct log_TELE_s {
-    uint8_t rssi;
+	uint8_t rssi;
-    uint8_t remote_rssi;
+	uint8_t remote_rssi;
-    uint8_t noise;
+	uint8_t noise;
-    uint8_t remote_noise;
+	uint8_t remote_noise;
-    uint16_t rxerrors;
+	uint16_t rxerrors;
-    uint16_t fixed;
+	uint16_t fixed;
-    uint8_t txbuf;
+	uint8_t txbuf;
 };
 
 /********** SYSTEM MESSAGES, ID > 0x80 **********/

src/systemcmds/dumpfile/dumpfile.c

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file dumpfile.c
+ *
+ * Dump file utility. Prints file size and contents in binary mode (don't replace LF with CR LF) to stdout.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <termios.h>
+
+#include <systemlib/err.h>
+
+__EXPORT int dumpfile_main(int argc, char *argv[]);
+
+int
+dumpfile_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		errx(1, "usage: dumpfile <filename>");
+	}
+
+	/* open input file */
+	FILE *f;
+	f = fopen(argv[1], "r");
+
+	if (f == NULL) {
+		printf("ERROR\n");
+		exit(1);
+	}
+
+	/* get file size */
+	fseek(f, 0L, SEEK_END);
+	int size = ftell(f);
+	fseek(f, 0L, SEEK_SET);
+
+	printf("OK %d\n", size);
+
+	/* configure stdout */
+	int out = fileno(stdout);
+
+	struct termios tc;
+	struct termios tc_old;
+	tcgetattr(out, &tc);
+
+	/* save old terminal attributes to restore it later on exit */
+	memcpy(&tc_old, &tc, sizeof(tc));
+
+	/* don't add CR on each LF*/
+	tc.c_oflag &= ~ONLCR;
+
+	if (tcsetattr(out, TCSANOW, &tc) < 0) {
+		warnx("ERROR setting stdout attributes");
+		exit(1);
+	}
+
+	char buf[512];
+	int nread;
+
+	/* dump file */
+	while ((nread = fread(buf, 1, sizeof(buf), f)) > 0) {
+		if (write(out, buf, nread) <= 0) {
+			warnx("error dumping file");
+			break;
+		}
+	}
+
+	fsync(out);
+	fclose(f);
+
+	/* restore old terminal attributes */
+	if (tcsetattr(out, TCSANOW, &tc_old) < 0) {
+		warnx("ERROR restoring stdout attributes");
+		exit(1);
+	}
+
+	return OK;
+}

src/systemcmds/dumpfile/module.mk

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2014 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.
+#
+############################################################################
+
+#
+# Dump file utility
+#
+
+MODULE_COMMAND	 = dumpfile
+SRCS		 = dumpfile.c
+
+MAXOPTIMIZATION	 = -Os