Merge remote-tracking branch 'upstream/master' into geo

36 files changed, 608 insertions, 131 deletions

diff --git a/.gitignore b/.gitignore
index 5b345b34a..c992dbf5a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -37,3 +37,4 @@ mavlink/include/mavlink/v0.9/
 tags
 .tags_sorted_by_file
 .pydevproject
+.ropeproject
diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
index 465a22c53..db0e40fc2 100644
--- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
+++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
@@ -5,4 +5,4 @@
 
 sh /etc/init.d/rc.fw_defaults
  
-set MIXER FMU_RET
+set MIXER easystar.mix
diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io
index 6e8fdbc0f..e23aebd87 100644
--- a/ROMFS/px4fmu_common/init.d/rc.io
+++ b/ROMFS/px4fmu_common/init.d/rc.io
@@ -11,7 +11,7 @@ px4io recovery
 # Adjust PX4IO update rate limit
 #
 set PX4IO_LIMIT 400
-if hw_ver compare PX4FMU_V1
+if ver hwcmp PX4FMU_V1
 then
 	set PX4IO_LIMIT 200
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index c5aef8273..3469d5f5f 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -5,7 +5,7 @@
 
 if [ -d /fs/microsd ]
 then
-	if hw_ver compare PX4FMU_V1
+	if ver hwcmp PX4FMU_V1
 	then
 		echo "Start sdlog2 at 50Hz"
 		sdlog2 start -r 50 -a -b 8 -t
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
index 4db62607a..a8c6dc811 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
@@ -32,9 +32,9 @@ then
 	param set MPC_Z_VEL_D 0.0
 	param set MPC_Z_VEL_MAX 3
 	param set MPC_Z_FF 0.5
-	param set MPC_TILT_MAX 1.0
+	param set MPC_TILTMAX_AIR 45.0
+	param set MPC_TILTMAX_LND 15.0
 	param set MPC_LAND_SPEED 1.0
-	param set MPC_LAND_TILT 0.3
 fi
 
 set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 235be2431..1e14930fe 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -22,7 +22,7 @@ then
 	echo "[init] Using L3GD20(H)"
 fi
 
-if hw_ver compare PX4FMU_V2
+if ver hwcmp PX4FMU_V2
 then
 	if lsm303d start
 	then
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 492afcb4c..756ee8ef8 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -248,7 +248,7 @@ then
 		echo "[init] ERROR: PX4IO not found, disabling output"
 		
 		# Avoid using ttyS0 for MAVLink on FMUv1
-		if hw_ver compare PX4FMU_V1
+		if ver hwcmp PX4FMU_V1
 		then
 			set FMU_MODE serial
 		fi
@@ -262,7 +262,7 @@ then
 	if [ $HIL == yes ]
 	then
 		set OUTPUT_MODE hil
-		if hw_ver compare PX4FMU_V1
+		if ver hwcmp PX4FMU_V1
 		then
 			set FMU_MODE serial
 		fi
@@ -308,7 +308,7 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 			
-			if hw_ver compare PX4FMU_V1
+			if ver hwcmp PX4FMU_V1
 			then
 				if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
 				then
@@ -383,7 +383,7 @@ then
 					tone_alarm $TUNE_OUT_ERROR
 				fi
 	
-				if hw_ver compare PX4FMU_V1
+				if ver hwcmp PX4FMU_V1
 				then
 					if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
 					then
diff --git a/ROMFS/px4fmu_common/mixers/easystar.mix b/ROMFS/px4fmu_common/mixers/easystar.mix
new file mode 100644
index 000000000..0051ffdbb
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/easystar.mix
@@ -0,0 +1,31 @@
+EASYSTAR / EASYSTAR II MIXER
+============================
+
+Aileron mixer
+-------------
+One output - would be easy to add support for 2 servos
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 0  10000  10000      0 -10000  10000
+
+Elevator mixer
+------------
+
+M: 1
+O:       10000   10000      0 -10000  10000
+S: 0 1  -10000  -10000      0 -10000  10000
+
+Rudder mixer
+------------
+
+M: 1
+O:       10000   10000      0 -10000  10000
+S: 0 2  -10000  -10000      0 -10000  10000
+
+Motor speed mixer
+-----------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000
diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py
index ceef9f9be..fcc40b09e 100644
--- a/Tools/px_romfs_pruner.py
+++ b/Tools/px_romfs_pruner.py
@@ -43,29 +43,30 @@ 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 
+
+        # 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:
+                        if ".zip" in file or ".bin" in file or ".swp" in file:
                                 continue
-                        
-                        file_path = os.path.join(root, file)                        
-                        
+
+                        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:
-                                        
+                                for line in f:
+
                                         # handle mixer files differently than startup files
                                         if file_path.endswith(".mix"):
                                                 if line.startswith(("Z:", "M:", "R: ", "O:", "S:")):
@@ -73,11 +74,11 @@ def main():
                                         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
+        main()
diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore
index 61e091551..87b314c61 100644
--- a/Tools/tests-host/.gitignore
+++ b/Tools/tests-host/.gitignore
@@ -1,2 +1,4 @@
 ./obj/*
 mixer_test
+sbus2_test
+autodeclination_test
diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile
index 7ab1454f0..f0737ef88 100644
--- a/Tools/tests-host/Makefile
+++ b/Tools/tests-host/Makefile
@@ -1,47 +1,39 @@
 
 CC=g++
-CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0"
+CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \
+	-I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm
 
-ODIR=obj
-LDIR =../lib
+all: mixer_test sbus2_test autodeclination_test
 
-LIBS=-lm
+MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \
+		../../src/systemcmds/tests/test_conv.cpp \
+		../../src/modules/systemlib/mixer/mixer_simple.cpp \
+		../../src/modules/systemlib/mixer/mixer_multirotor.cpp \
+		../../src/modules/systemlib/mixer/mixer.cpp \
+		../../src/modules/systemlib/mixer/mixer_group.cpp \
+		../../src/modules/systemlib/mixer/mixer_load.c \
+		../../src/modules/systemlib/pwm_limit/pwm_limit.c \
+		hrt.cpp \
+		mixer_test.cpp
 
-#_DEPS = test.h
-#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS))
+SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \
+		hrt.cpp \
+		sbus2_test.cpp
 
-_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \
-	mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o
-OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ))
+AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \
+		hrt.cpp \
+		autodeclination_test.cpp
 
-#$(DEPS)
-$(ODIR)/%.o: %.cpp
-	mkdir -p obj
-	$(CC) -c -o $@ $< $(CFLAGS)
+mixer_test: $(MIXER_FILES)
+	$(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS)
 
-$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp
-	$(CC) -c -o $@ $< $(CFLAGS)
+sbus2_test: $(SBUS2_FILES)
+	$(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS)
 
-$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
-	$(CC) -c -o $@ $< $(CFLAGS)
-
-$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
-	$(CC) -c -o $@ $< $(CFLAGS)
-
-$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp
-	$(CC) -c -o $@ $< $(CFLAGS)
-
-$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c
-	$(CC) -c -o $@ $< $(CFLAGS)
-
-$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c
-	$(CC) -c -o $@ $< $(CFLAGS)
-
-#
-mixer_test: $(OBJ)
-	g++ -o $@ $^ $(CFLAGS) $(LIBS)
+autodeclination_test: $(SBUS2_FILES)
+	$(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS)
 
 .PHONY: clean
 
 clean:
-	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~
\ No newline at end of file
+	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test
\ No newline at end of file
diff --git a/Tools/tests-host/autodeclination_test.cpp b/Tools/tests-host/autodeclination_test.cpp
new file mode 100644
index 000000000..93bc340bb
--- /dev/null
+++ b/Tools/tests-host/autodeclination_test.cpp
@@ -0,0 +1,28 @@
+
+#include <stdio.h>
+#include <stdlib.h> 
+#include <unistd.h>
+#include <string.h>
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <px4iofirmware/px4io.h>
+#include "../../src/systemcmds/tests/tests.h"
+#include <geo/geo.h>
+
+int main(int argc, char *argv[]) {
+	warnx("autodeclination test started");
+
+	if (argc < 3)
+		errx(1, "Need lat/lon!");
+
+	char* p_end;
+
+	float lat = strtod(argv[1], &p_end);
+	float lon = strtod(argv[2], &p_end);
+
+	float declination = get_mag_declination(lat, lon);
+
+	printf("lat: %f lon: %f, dec: %f\n", lat, lon, declination);
+
+}
diff --git a/Tools/tests-host/board_config.h b/Tools/tests-host/board_config.h
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/Tools/tests-host/board_config.h
diff --git a/Tools/tests-host/debug.h b/Tools/tests-host/debug.h
new file mode 100644
index 000000000..9824d13fc
--- /dev/null
+++ b/Tools/tests-host/debug.h
@@ -0,0 +1,5 @@
+
+#pragma once
+
+#include <systemlib/err.h>
+#define lowsyslog warnx
\ No newline at end of file
diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp
index e311617f9..06499afd0 100644
--- a/Tools/tests-host/mixer_test.cpp
+++ b/Tools/tests-host/mixer_test.cpp
@@ -11,4 +11,4 @@ int main(int argc, char *argv[]) {
 	test_mixer(3, args);
 
 	test_conv(1, args);
-}
\ No newline at end of file
+}
diff --git a/Tools/tests-host/run_tests.sh b/Tools/tests-host/run_tests.sh
new file mode 100755
index 000000000..ff5ee509a
--- /dev/null
+++ b/Tools/tests-host/run_tests.sh
@@ -0,0 +1,6 @@
+#!/bin/sh
+
+make clean
+make all
+./mixer_test
+./sbus2_test ../../../../data/sbus2/sbus2_r7008SB_gps_baro_tx_off.txt
\ No newline at end of file
diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp
new file mode 100644
index 000000000..d8fcb695d
--- /dev/null
+++ b/Tools/tests-host/sbus2_test.cpp
@@ -0,0 +1,75 @@
+
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <px4iofirmware/px4io.h>
+#include "../../src/systemcmds/tests/tests.h"
+
+int main(int argc, char *argv[]) {
+	warnx("SBUS2 test started");
+
+	if (argc < 2)
+		errx(1, "Need a filename for the input file");
+
+	warnx("loading data from: %s", argv[1]);
+
+	FILE *fp;
+	
+	fp = fopen(argv[1],"rt");
+
+	if (!fp)
+		errx(1, "failed opening file");
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		(void)fscanf(fp, "%f,%x,,", &f, &x);
+	}
+
+	// Init the parser
+	uint8_t frame[30];
+	unsigned partial_frame_count = 0;
+	uint16_t rc_values[18];
+	uint16_t num_values;
+	bool sbus_failsafe;
+	bool sbus_frame_drop;
+	uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
+
+	float last_time = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+		if (((f - last_time) * 1000 * 1000) > 3000) {
+			partial_frame_count = 0;
+			warnx("FRAME RESET\n\n");
+		}
+
+		frame[partial_frame_count] = x;
+		partial_frame_count++;
+
+		//warnx("%f: 0x%02x, first: 0x%02x, last: 0x%02x, pcount: %u", (double)f, x, frame[0], frame[24], partial_frame_count);
+
+		if (partial_frame_count == sizeof(frame))
+			partial_frame_count = 0;
+
+		last_time = f;
+
+		// Pipe the data into the parser
+		hrt_abstime now = hrt_absolute_time();
+
+		//if (partial_frame_count % 25 == 0)
+			//sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels);
+	}
+
+	if (ret == EOF) {
+		warnx("Test finished, reached end of file");
+	} else {
+		warnx("Test aborted, errno: %d", ret);
+	}
+
+}
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 532e978d0..61a417f30 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -55,8 +55,8 @@ MODULES		+= systemcmds/top
 MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
-MODULES		+= systemcmds/hw_ver
 MODULES		+= systemcmds/dumpfile
+MODULES		+= systemcmds/ver
 
 #
 # General system control
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index e13421acc..65ca24325 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -63,8 +63,8 @@ MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
-MODULES		+= systemcmds/hw_ver
 MODULES		+= systemcmds/dumpfile
+MODULES		+= systemcmds/ver
 
 #
 # General system control
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 79922374d..84274bf75 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -29,7 +29,7 @@ MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
-MODULES		+= systemcmds/hw_ver
+MODULES		+= systemcmds/ver
 
 #
 # Library modules
diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk
index 1b646d9e0..60602e76f 100644
--- a/makefiles/firmware.mk
+++ b/makefiles/firmware.mk
@@ -113,7 +113,7 @@ endif
 $(info %  GIT_DESC            = $(GIT_DESC))
 
 #
-# Set a default target so that included makefiles or errors here don't 
+# Set a default target so that included makefiles or errors here don't
 # cause confusion.
 #
 # XXX We could do something cute here with $(DEFAULT_GOAL) if it's not one
@@ -177,7 +177,7 @@ GLOBAL_DEPS		+= $(MAKEFILE_LIST)
 #
 # Extra things we should clean
 #
-EXTRA_CLEANS		 = 
+EXTRA_CLEANS		 =
 
 
 #
@@ -371,6 +371,8 @@ $(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS)
 $(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS)
 	$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)
 	$(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH)
+# delete all files in ROMFS_SCRATCH which start with a . or end with a ~
+	$(Q) $(RM) $(ROMFS_SCRATCH)/*/.[!.]* $(ROMFS_SCRATCH)/*/*~
 ifneq ($(ROMFS_EXTRA_FILES),)
 	$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras
 	$(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras
diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h
index d130d68b3..8bfc90c64 100644
--- a/src/drivers/drv_hrt.h
+++ b/src/drivers/drv_hrt.h
@@ -41,6 +41,7 @@
 
 #include <sys/types.h>
 #include <stdbool.h>
+#include <inttypes.h>
 
 #include <time.h>
 #include <queue.h>
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 3d7ba050f..1d0cd524a 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -50,6 +50,8 @@
 
 __BEGIN_DECLS
 
+#include "geo/geo_mag_declination.h"
+
 #include <stdbool.h>
 
 #define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c
new file mode 100644
index 000000000..09eac38f4
--- /dev/null
+++ b/src/lib/geo/geo_mag_declination.c
@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 MAV GEO Library (MAVGEO). 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 MAVGEO 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 geo_mag_declination.c
+*
+* Calculation / lookup table for earth magnetic field declination.
+* 
+* Lookup table from Scott Ferguson <scottfromscott@gmail.com>
+*
+* XXX Lookup table currently too coarse in resolution (only full degrees)
+* and lat/lon res - needs extension medium term.
+*
+*/
+
+#include <geo/geo.h>
+
+/** set this always to the sampling in degrees for the table below */
+#define SAMPLING_RES		10.0f
+#define SAMPLING_MIN_LAT	-60.0f
+#define SAMPLING_MAX_LAT	60.0f
+#define SAMPLING_MIN_LON	-180.0f
+#define SAMPLING_MAX_LON	180.0f
+
+static const int8_t declination_table[13][37] = \
+{
+	46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \
+	-66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \
+	-3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \
+	29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \
+	-40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \
+	8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \
+	10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \
+	-10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \
+	-14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \
+	9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \
+	7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \
+	0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \
+	0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \
+	0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \
+	-16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \
+	12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \
+	0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \
+	13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3
+};
+
+static float get_lookup_table_val(unsigned lat, unsigned lon);
+
+__EXPORT float get_mag_declination(float lat, float lon)
+{
+	/*
+	 * If the values exceed valid ranges, return zero as default
+	 * as we have no way of knowing what the closest real value
+	 * would be.
+	 */
+	if (lat < -90.0f || lat > 90.0f ||
+	    lon < -180.0f || lon > 180.0f) {
+		return 0.0f;
+	}
+
+	/* round down to nearest sampling resolution */
+	int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES;
+	int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES;
+
+	/* for the rare case of hitting the bounds exactly
+	 * the rounding logic wouldn't fit, so enforce it.
+	 */
+
+	/* limit to table bounds - required for maxima even when table spans full globe range */
+	if (lat <= SAMPLING_MIN_LAT) {
+		min_lat = SAMPLING_MIN_LAT;
+	}
+
+	if (lat >= SAMPLING_MAX_LAT) {
+		min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
+	}
+
+	if (lon <= SAMPLING_MIN_LON) {
+		min_lon = SAMPLING_MIN_LON;
+	}
+
+	if (lon >= SAMPLING_MAX_LON) {
+		min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
+	}
+
+	/* find index of nearest low sampling point */
+	unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat)  / SAMPLING_RES;
+	unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES;
+
+	float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index);
+	float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1);
+	float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1);
+	float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index);
+
+	/* perform bilinear interpolation on the four grid corners */
+
+	float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw;
+	float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw;
+
+	return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min;
+}
+
+float get_lookup_table_val(unsigned lat_index, unsigned lon_index)
+{
+	return declination_table[lat_index][lon_index];
+}
\ No newline at end of file
diff --git a/src/systemcmds/hw_ver/hw_ver.c b/src/lib/geo/geo_mag_declination.h
index 4b84523cc..0ac062d6d 100644
--- a/src/systemcmds/hw_ver/hw_ver.c
+++ b/src/lib/geo/geo_mag_declination.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name MAVGEO nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -32,42 +32,16 @@
  ****************************************************************************/
 
 /**
- * @file hw_ver.c
- *
- * Show and test hardware version.
- */
-
-#include <nuttx/config.h>
-
-#include <stdio.h>
-#include <string.h>
-#include <errno.h>
-#include <version/version.h>
-
-__EXPORT int hw_ver_main(int argc, char *argv[]);
+* @file geo_mag_declination.h
+*
+* Calculation / lookup table for earth magnetic field declination.
+*
+*/
 
-int
-hw_ver_main(int argc, char *argv[])
-{
-	if (argc >= 2) {
-		if (!strcmp(argv[1], "show")) {
-			printf(HW_ARCH "\n");
-			exit(0);
-		}
+#pragma once
 
-		if (!strcmp(argv[1], "compare")) {
-			if (argc >= 3) {
-				int ret = strcmp(HW_ARCH, argv[2]) != 0;
-				if (ret == 0) {
-					printf("hw_ver match: %s\n", HW_ARCH);
-				}
-				exit(ret);
+__BEGIN_DECLS
 
-			} else {
-				errx(1, "not enough arguments, try 'compare PX4FMU_1'");
-			}
-		}
-	}
+__EXPORT float get_mag_declination(float lat, float lon);
 
-	errx(1, "expected a command, try 'show' or 'compare'");
-}
+__END_DECLS
diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk
index 30a2dc99f..9500a2bcc 100644
--- a/src/lib/geo/module.mk
+++ b/src/lib/geo/module.mk
@@ -35,4 +35,5 @@
 # Geo library
 #
 
-SRCS		 = geo.c
+SRCS		=	geo.c \
+			geo_mag_declination.c
diff --git a/src/lib/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h
index 6f66f9ee3..61d3a3b61 100644
--- a/src/lib/mathlib/CMSIS/Include/arm_math.h
+++ b/src/lib/mathlib/CMSIS/Include/arm_math.h
@@ -5193,7 +5193,7 @@ void arm_rfft_fast_f32(
     *pIa = Ialpha;

 

     /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */

-    *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;

+    *pIb = (float32_t)-0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;

 

   }

 

diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index fe6c9bfaa..0fd3c9e9e 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -199,15 +199,9 @@ int led_init()
 	}
 
 	/* the blue LED is only available on FMUv1 but not FMUv2 */
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-
-	if (ioctl(leds, LED_ON, LED_BLUE)) {
-		warnx("Blue LED: ioctl fail\n");
-		return ERROR;
-	}
-
-#endif
+	(void)ioctl(leds, LED_ON, LED_BLUE);
 
+	/* we consider the amber led mandatory */
 	if (ioctl(leds, LED_ON, LED_AMBER)) {
 		warnx("Amber LED: ioctl fail\n");
 		return ERROR;
@@ -217,11 +211,7 @@ int led_init()
 	rgbleds = open(RGBLED_DEVICE_PATH, 0);
 
 	if (rgbleds == -1) {
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
-		errx(1, "Unable to open " RGBLED_DEVICE_PATH);
-#else
-		warnx("No RGB LED found");
-#endif
+		warnx("No RGB LED found at " RGBLED_DEVICE_PATH);
 	}
 
 	return 0;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 678ce1645..bef8a5a55 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -819,11 +819,11 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		bool updated = status_sub->update(t);
-		updated |= pos_sp_triplet_sub->update(t);
-		updated |= act_sub->update(t);
+		bool updated = act_sub->update(t);
+		(void)pos_sp_triplet_sub->update(t);
+		(void)status_sub->update(t);
 
-		if (updated) {
+		if (updated && (status->arming_state == ARMING_STATE_ARMED)) {
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state;
 			uint8_t mavlink_base_mode;
@@ -1353,7 +1353,7 @@ protected:
 		uint8_t orientation = 0;
 		uint8_t covariance = 20;
 
-		mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, 
+		mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
 			range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
 	}
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index ddbf4fc69..1cfbde1d2 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -432,8 +432,8 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	memset(&manual, 0, sizeof(manual));
 
 	manual.timestamp = hrt_absolute_time();
-	manual.roll = man.x / 1000.0f;
-	manual.pitch = man.y / 1000.0f;
+	manual.pitch = man.x / 1000.0f;
+	manual.roll = man.y / 1000.0f;
 	manual.yaw = man.r / 1000.0f;
 	manual.throttle = man.z / 1000.0f;
 
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 6b0c44fb3..36d95bf06 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -496,7 +496,8 @@ MulticopterAttitudeControl::control_attitude(float dt)
 			//}
 		} else {
 			/* move yaw setpoint */
-			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_yaw_max * dt);
+			yaw_sp_move_rate = _manual_control_sp.yaw * _params.man_yaw_max;
+			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
 			_v_att_sp.R_valid = false;
 			publish_att_sp = true;
 		}
diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c
index b05273c0d..bf3428a50 100644
--- a/src/modules/systemlib/mixer/mixer_load.c
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -41,6 +41,7 @@
 #include <string.h>
 #include <stdio.h>
 #include <ctype.h>
+#include <systemlib/err.h>
 
 #include "mixer_load.h"
 
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index ad1996694..7d80af307 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -182,7 +182,7 @@ esc_calib_main(int argc, char *argv[])
 
 	if (orb_updated) {
 		errx(1, "ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n"
-			"\tmultirotor_att_control stop\n"
+			"\tmc_att_control stop\n"
 			"\tfw_att_control stop\n");
 	}
 
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 7c23f38c2..1828c660f 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 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
@@ -443,6 +443,110 @@ pwm_main(int argc, char *argv[])
 					exit(0);
 				}
 			}
+			usleep(2000);
+		}
+		exit(0);
+
+
+	} else if (!strcmp(argv[1], "steps")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
+		}
+
+		/* get current servo values */
+		struct pwm_output_values last_spos;
+
+		for (unsigned i = 0; i < servo_count; i++) {
+
+			ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]);
+			if (ret != OK)
+				err(1, "PWM_SERVO_GET(%d)", i);
+		}
+
+		/* perform PWM output */
+
+		/* Open console directly to grab CTRL-C signal */
+		struct pollfd fds;
+		fds.fd = 0; /* stdin */
+		fds.events = POLLIN;
+
+		warnx("Running 5 steps. WARNING! Motors will be live in 5 seconds\nPress any key to abort now.");
+		sleep(5);
+
+		unsigned off = 900;
+		unsigned idle = 1300;
+		unsigned full = 2000;
+		unsigned steps_timings_us[] = {2000, 5000, 20000, 50000};
+
+		unsigned phase = 0;
+		unsigned phase_counter = 0;
+		unsigned const phase_maxcount = 20;
+
+		for (	unsigned steps_timing_index = 0;
+			steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]);
+			steps_timing_index++ ) {
+
+			warnx("Sending step input with 0 to 100%% over a %u microseconds ramp", steps_timings_us[steps_timing_index]);
+
+			while (1) {
+				for (unsigned i = 0; i < servo_count; i++) {
+					if (set_mask & 1<<i) {
+
+						unsigned val;
+
+						if (phase == 0) {
+							val = idle;
+						} else if (phase == 1) {
+							/* ramp - depending how steep it is this ramp will look instantaneous on the output */
+							val = idle + (full - idle) * (phase_maxcount / (float)phase_counter);
+						} else {
+							val = off;
+						}
+
+						ret = ioctl(fd, PWM_SERVO_SET(i), val);
+						if (ret != OK)
+							err(1, "PWM_SERVO_SET(%d)", i);
+					}
+				}
+
+				/* abort on user request */
+				char c;
+				ret = poll(&fds, 1, 0);
+				if (ret > 0) {
+
+				int ret = read(0, &c, 1);
+					if (ret > 0) {
+						/* reset output to the last value */
+						for (unsigned i = 0; i < servo_count; i++) {
+										if (set_mask & 1<<i) {
+											ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
+											if (ret != OK)
+												err(1, "PWM_SERVO_SET(%d)", i);
+										}
+									}
+						warnx("Key pressed, user abort\n");
+						exit(0);
+					}
+				}
+				if (phase == 1) {
+					usleep(steps_timings_us[steps_timing_index] / phase_maxcount);
+
+				} else if (phase == 0) {
+					usleep(50000);
+				} else if (phase == 2) {
+					usleep(50000);
+				} else {
+					break;
+				}
+
+				phase_counter++;
+
+				if (phase_counter > phase_maxcount) {
+					phase++;
+					phase_counter = 0;
+				}
+			}
 		}
 		exit(0);
 
diff --git a/src/systemcmds/hw_ver/module.mk b/src/systemcmds/ver/module.mk
index 3cc08b6a1..2eeb80b61 100644
--- a/src/systemcmds/hw_ver/module.mk
+++ b/src/systemcmds/ver/module.mk
@@ -32,12 +32,13 @@
 ############################################################################
 
 #
-# Show and test hardware version
+# "version" nsh-command displays version infromation for hw,sw, gcc,build etc
+# can be also included and used in own code via "ver.h"
 #
 
-MODULE_COMMAND	 = hw_ver
-SRCS		 = hw_ver.c
+MODULE_COMMAND	 	= ver
+SRCS		 		= ver.c
 
-MODULE_STACKSIZE = 1024
+MODULE_STACKSIZE 	= 1024
 
-MAXOPTIMIZATION	 = -Os
+MAXOPTIMIZATION	 	= -Os
diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
new file mode 100644
index 000000000..9ae080ee2
--- /dev/null
+++ b/src/systemcmds/ver/ver.c
@@ -0,0 +1,123 @@
+/****************************************************************************
+*
+* 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 ver.c
+*
+* Version command, unifies way of showing versions of HW, SW, Build, GCC
+* In case you want to add new version just extend version_main function
+*
+* @author Vladimir Kulla <ufon@kullaonline.net>
+*/
+
+#include <stdio.h>
+#include <string.h>
+#include <version/version.h>
+#include <systemlib/err.h>
+
+// string constants for version commands
+static const char sz_ver_hw_str[] 	= "hw";
+static const char sz_ver_hwcmp_str[] = "hwcmp";
+static const char sz_ver_git_str[] 	= "git";
+static const char sz_ver_bdate_str[] = "bdate";
+static const char sz_ver_gcc_str[] 	= "gcc";
+static const char sz_ver_all_str[] 	= "all";
+
+static void usage(const char *reason)
+{
+	if (reason != NULL) {
+		printf("%s\n", reason);
+	}
+
+	printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n");
+}
+
+__EXPORT int ver_main(int argc, char *argv[]);
+
+int ver_main(int argc, char *argv[])
+{
+	int ret = 1;	//defaults to an error
+
+	// first check if there are at least 2 params
+	if (argc >= 2) {
+		if (argv[1] != NULL) {
+			if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
+				printf("%s\n", HW_ARCH);
+				ret = 0;
+
+			} else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
+				if (argc >= 3 && argv[2] != NULL) {
+					// compare 3rd parameter with HW_ARCH string, in case of match, return 0
+					ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH));
+
+					if (ret == 0) {
+						printf("ver hwcmp match: %s\n", HW_ARCH);
+					}
+
+				} else {
+					errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'");
+				}
+
+			} else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
+				printf("%s\n", FW_GIT);
+				ret = 0;
+
+			} else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
+				printf("%s %s\n", __DATE__, __TIME__);
+				ret = 0;
+
+			} else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
+				printf("%s\n", __VERSION__);
+				ret = 0;
+
+			} else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) {
+				printf("HW arch: %s\n", HW_ARCH);
+				printf("Build datetime: %s %s\n", __DATE__, __TIME__);
+				printf("FW git-hash: %s\n", FW_GIT);
+				printf("GCC toolchain: %s\n", __VERSION__);
+				ret = 0;
+
+			} else {
+				errx(1, "unknown command.\n");
+			}
+
+		} else {
+			usage("Error, input parameter NULL.\n");
+		}
+
+	} else {
+		usage("Error, not enough parameters.");
+	}
+
+	return ret;
+}