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

45 files changed, 935 insertions, 344 deletions

diff --git a/.travis.yml b/.travis.yml
index 9386ddaa3..a994d3471 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -52,20 +52,17 @@ script:
   - echo -en 'travis_fold:end:script.3\\r'
   - zip Firmware.zip Images/*.px4
 
-# We use an encrypted env variable to ensure this only executes when artifacts are uploaded.
-#after_script:
-#  - echo "Branch $TRAVIS_BRANCH (pull request: $TRAVIS_PULL_REQUEST) ready for flight testing." >> $PX4_REPORT
-#  - git log -n1 > $PX4_REPORT
-#  - echo " " >> $PX4_REPORT
-#  - echo "Files available at:" >> $PX4_REPORT
-#  - echo "https://px4-travis.s3.amazonaws.com/PX4/Firmware/$TRAVIS_BUILD_NUMBER/$TRAVIS_BUILD_NUMBER.1/Firmware.zip" >> $PX4_REPORT
-#  - echo "Description of desired tests is available at:" >> $PX4_REPORT
-#  - echo "https://github.com/PX4/Firmware/pull/$TRAVIS_PULL_REQUEST" >> $PX4_REPORT
-#  - echo " " >> $PX4_REPORT
-#  - echo "Thanks for testing!" >> $PX4_REPORT
-#  - echo " " >> $PX4_REPORT
-#  - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < "$PX4_REPORT"
-#  - s3cmd put --acl-public --guess-mime-type --config=.s3cfg Firmware.zip s3://s3-website-us-east-1.amazonaws.com/#$TRAVIS_JOB_ID/
+after_script:
+  - git clone git://github.com/PX4/CI-Tools.git
+  - ./CI-Tools/s3cmd-configure
+# upload newest build for this branch with s3 index
+  - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
+# archive newest build by date with s3 index
+  - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/
+  - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/
+# upload top level index.html and timestamp.html
+  - ./CI-Tools/s3cmd-put CI-Tools/index.html index.html
+  - ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html
 
 deploy:
   provider: releases
@@ -78,18 +75,6 @@ deploy:
     all_branches: true
     repo: PX4/Firmware
 
-addons:
-  artifacts:
-    paths:
-    - "Firmware.zip"
-    key:
-      secure: j4y9x9KXUiarGrnpFBLPIkEKIH8X6oSRUO61TwxTOamsE0eEKnIaCz1Xq83q7DoqzomHBD3qXAFPV9dhLr1zdKEPJDIyV45GVD4ClIQIzh/P3Uc7kDNxKzdmxY12SH6D0orMpC4tCf1sNK7ETepltWfcnjaDk1Rjs9+TVY7LuzM=
-    secret:
-      secure: CJC7VPGtEhJu8Pix85iPF8xUvMPZvTgnHyd9MrSlPKCFFMrlgz9eMT0WWW/TPQ+s4LPwJIfEQx2Q0BRT5tOXuvsTLuOG68mplVddhTWbHb0m0qTQErXFHEppvW4ayuSdeLJ4TjTWphBVainL0mcLLRwQfuAJJDDs/sGan3WrG+Y=
-    bucket: px4-travis
-    region: us-east-1
-    endpoint: s3-website-us-east-1.amazonaws.com
-
 notifications:
   webhooks:
     urls:
diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py
index cf86dd668..826e12c97 100644
--- a/Debug/Nuttx.py
+++ b/Debug/Nuttx.py
@@ -1,7 +1,10 @@
 # GDB/Python functions for dealing with NuttX
 
+from __future__ import print_function
 import gdb, gdb.types
 
+parse_int = lambda x: int(str(x), 0)
+
 class NX_register_set(object):
 	"""Copy of the registers for a given context"""
 
@@ -155,7 +158,7 @@ class NX_task(object):
 		result = []
 		for i in range(pidhash_type.range()[0],pidhash_type.range()[1]):
 			entry = pidhash_value[i]
-			pid = int(entry['pid'])
+			pid = parse_int(entry['pid'])
 			if pid is not -1:
 				result.append(pid)
 		return result
@@ -184,7 +187,7 @@ class NX_task(object):
 				self.__dict__['stack_used'] = 0
 			else:
 				stack_limit = self._tcb['adj_stack_size']
-				for offset in range(0, int(stack_limit)):
+				for offset in range(0, parse_int(stack_limit)):
 					if stack_base[offset] != 0xff:
 						break
 				self.__dict__['stack_used'] = stack_limit - offset
@@ -244,7 +247,7 @@ class NX_task(object):
 		filearray = filelist['fl_files']
 		result = dict()
 		for i in range(filearray.type.range()[0],filearray.type.range()[1]):
-			inode = int(filearray[i]['f_inode'])
+			inode = parse_int(filearray[i]['f_inode'])
 			if inode != 0:
 				result[i] = inode
 		return result
@@ -299,7 +302,7 @@ class NX_show_task (gdb.Command):
 		super(NX_show_task, self).__init__("show task", gdb.COMMAND_USER)
 
 	def invoke(self, arg, from_tty):
-		t = NX_task.for_pid(int(arg))
+		t = NX_task.for_pid(parse_int(arg))
 		if t is not None:
 			my_fmt = 'PID:{pid}  name:{name}  state:{state}\n'
 			my_fmt += '  stack used {stack_used} of {stack_limit}\n'
@@ -435,12 +438,12 @@ class NX_tcb(object):
 		first_tcb = tcb_ptr.dereference()
 		tcb_list.append(first_tcb);
 		next_tcb = first_tcb['flink'].dereference()
-		while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]):
+		while not self.is_in(parse_int(next_tcb['pid']),[parse_int(t['pid']) for t in tcb_list]):
 			tcb_list.append(next_tcb); 
 			old_tcb = next_tcb;
 			next_tcb = old_tcb['flink'].dereference()
 		
-		return [t for t in tcb_list if int(t['pid'])<2000]
+		return [t for t in tcb_list if parse_int(t['pid'])<2000]
 	
 	def getTCB(self):
 		list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks']
@@ -469,12 +472,12 @@ class NX_check_stack_order(gdb.Command):
 		first_tcb = tcb_ptr.dereference()
 		tcb_list.append(first_tcb);
 		next_tcb = first_tcb['flink'].dereference()
-		while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]):
+		while not self.is_in(parse_int(next_tcb['pid']),[parse_int(t['pid']) for t in tcb_list]):
 			tcb_list.append(next_tcb); 
 			old_tcb = next_tcb;
 			next_tcb = old_tcb['flink'].dereference()
 		
-		return [t for t in tcb_list if int(t['pid'])<2000]
+		return [t for t in tcb_list if parse_int(t['pid'])<2000]
 	
 	def getTCB(self):
 		list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks']
@@ -488,7 +491,7 @@ class NX_check_stack_order(gdb.Command):
 	def getSPfromTask(self,tcb):
 		regmap = NX_register_set.v7em_regmap
 		a =tcb['xcp']['regs']
-		return 	int(a[regmap['SP']])
+		return 	parse_int(a[regmap['SP']])
 	
 	def find_closest(self,list,val):
 		tmp_list = [abs(i-val) for i in list]
@@ -525,8 +528,8 @@ class NX_check_stack_order(gdb.Command):
 		for t in tcb:
 			p = [];
 			#print(t.name,t._tcb['stack_alloc_ptr'])
-			p.append(int(t['stack_alloc_ptr']))
-			p.append(int(t['adj_stack_ptr']))
+			p.append(parse_int(t['stack_alloc_ptr']))
+			p.append(parse_int(t['adj_stack_ptr']))
 			p.append(self.getSPfromTask(t))
 			stackadresses[str(t['name'])] = p;
 		address = int("0x30000000",0)
@@ -594,12 +597,12 @@ class NX_search_tcb(gdb.Command):
 		first_tcb = tcb_ptr.dereference()
 		tcb_list.append(first_tcb);
 		next_tcb = first_tcb['flink'].dereference()
-		while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]):
+		while not self.is_in(parse_int(next_tcb['pid']),[parse_int(t['pid']) for t in tcb_list]):
 			tcb_list.append(next_tcb); 
 			old_tcb = next_tcb;
 			next_tcb = old_tcb['flink'].dereference()
 		
-		return [t for t in tcb_list if int(t['pid'])<2000]
+		return [t for t in tcb_list if parse_int(t['pid'])<2000]
 	
 	def invoke(self,args,sth):
 		list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks']
@@ -612,7 +615,7 @@ class NX_search_tcb(gdb.Command):
 		# filter for tasks that are listed twice
 		tasks_filt = {}
 		for t in tasks:
-			pid = int(t['pid']);
+			pid = parse_int(t['pid']);
 			if not pid in tasks_filt.keys():
 				tasks_filt[pid] = t['name']; 
 		print('{num_t} Tasks found:'.format(num_t = len(tasks_filt)))
@@ -653,62 +656,49 @@ class NX_my_bt(gdb.Command):
 		tcb_ptr = addr_value.cast(gdb.lookup_type('struct tcb_s').pointer())
 		return tcb_ptr.dereference()
 	
-	def print_instruction_at(self,addr,stack_percentage):
+	def resolve_file_line_func(self,addr,stack_percentage):
 		gdb.write(str(round(stack_percentage,2))+":")
 		str_to_eval = "info line *"+hex(addr)
 		#gdb.execute(str_to_eval)
 		res = gdb.execute(str_to_eval,to_string = True)
 		# get information from results string:
 		words = res.split()
-		valid = False
-		if words[0] == 'No':
-			#no line info...
-			pass
-		else:
-			valid = True
+		if words[0] != 'No':
 			line = int(words[1])
-			idx = words[3].rfind("/"); #find first backslash
-			if idx>0:
-				name = words[3][idx+1:];
-				path = words[3][:idx];
-			else:
-				name = words[3];
-				path = "";
 			block = gdb.block_for_pc(addr)
 			func = block.function
 			if str(func) == "None":
 				func = block.superblock.function
-			
-		if valid:
-			print("Line: ",line," in ",path,"/",name,"in ",func)
-			return name,path,line,func
-			
-			
-			
+			return words[3].strip('"'), line, func
 		
 	def invoke(self,args,sth):
-		addr_dec = int(args[2:],16)
-		_tcb = self.get_tcb_from_address(addr_dec)
+		try:
+			addr_dec = parse_int(args)   # Trying to interpret the input as TCB address
+		except ValueError:
+			for task in NX_task.tasks(): # Interpreting as a task name
+				if task.name == args:
+					_tcb = task._tcb
+					break
+		else:
+			_tcb = self.get_tcb_from_address(addr_dec)
+
 		print("found task with PID: ",_tcb["pid"])
-		up_stack = int(_tcb['adj_stack_ptr'])
-		curr_sp = int(_tcb['xcp']['regs'][0]) #curr stack pointer
-		other_sp = int(_tcb['xcp']['regs'][8]) # other stack pointer
-		stacksize = int(_tcb['adj_stack_size']) # other stack pointer
+		up_stack = parse_int(_tcb['adj_stack_ptr'])
+		curr_sp = parse_int(_tcb['xcp']['regs'][0]) #curr stack pointer
+		other_sp = parse_int(_tcb['xcp']['regs'][8]) # other stack pointer
+		stacksize = parse_int(_tcb['adj_stack_size']) # other stack pointer
 		
 		print("tasks current SP = ",hex(curr_sp),"stack max ptr is at ",hex(up_stack))
 		
-		if curr_sp == up_stack:
-			sp = other_sp
-		else: 
-			sp = curr_sp;
-			
-		while(sp < up_stack):
+		item = 0
+		for sp in range(other_sp if curr_sp == up_stack else curr_sp, up_stack, 4):
 			mem = self.readmem(sp)
 			#print(hex(sp)," : ",hex(mem))
 			if self.is_in_bounds(mem):
 				# this is a potential instruction ptr
 				stack_percentage = (up_stack-sp)/stacksize
-				name,path,line,func = self.print_instruction_at(mem,stack_percentage)
-			sp = sp + 4; # jump up one word
+				filename,line,func = self.resolve_file_line_func(mem, stack_percentage)
+				print('#%-2d ' % item, '0x%08x in ' % mem, func, ' at ', filename, ':', line, sep='')
+				item += 1
 		
 NX_my_bt()
diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh
new file mode 100755
index 000000000..ab06a1b66
--- /dev/null
+++ b/Debug/poor-mans-profiler.sh
@@ -0,0 +1,260 @@
+#!/bin/bash
+#
+# Poor man's sampling profiler for NuttX.
+#
+# Usage: Install flamegraph.pl in your PATH, configure your .gdbinit, run the script with proper arguments and go
+#        have a coffee. When you're back, you'll see the flamegraph. Note that frequent calls to GDB significantly
+#        interfere with normal operation of the target, which means that you can't profile real-time tasks with it.
+#
+# Requirements: ARM GDB with Python support
+#
+
+set -e
+root=$(dirname $0)/..
+
+function die()
+{
+    echo "$@"
+    exit 1
+}
+
+function usage()
+{
+    echo "Invalid usage. Supported options:"
+    cat $0 | sed -n 's/^\s*--\([^)\*]*\).*/\1/p' # Don't try this at home.
+    exit 1
+}
+
+which flamegraph.pl > /dev/null || die "Install flamegraph.pl first"
+
+#
+# Parsing the arguments. Read this section for usage info.
+#
+nsamples=0
+sleeptime=0.1    # Doctors recommend 7-8 hours a day
+taskname=
+elf=$root/Build/px4fmu-v2_default.build/firmware.elf
+append=0
+fgfontsize=10
+fgwidth=1900
+
+for i in "$@"
+do
+    case $i in
+        --nsamples=*)
+            nsamples="${i#*=}"
+            ;;
+        --sleeptime=*)
+            sleeptime="${i#*=}"
+            ;;
+        --taskname=*)
+            taskname="${i#*=}"
+            ;;
+        --elf=*)
+            elf="${i#*=}"
+            ;;
+        --append)
+            append=1
+            ;;
+        --fgfontsize=*)
+            fgfontsize="${i#*=}"
+            ;;
+        --fgwidth=*)
+            fgwidth="${i#*=}"
+            ;;
+        *)
+            usage
+            ;;
+    esac
+    shift
+done
+
+#
+# Temporary files
+#
+stacksfile=/tmp/pmpn-stacks.log
+foldfile=/tmp/pmpn-folded.txt
+graphfile=/tmp/pmpn-flamegraph.svg
+gdberrfile=/tmp/pmpn-gdberr.log
+
+#
+# Sampling if requested. Note that if $append is true, the stack file will not be rewritten.
+#
+cd $root
+
+if [[ $nsamples > 0 ]]
+then
+    [[ $append = 0 ]] && (rm -f $stacksfile; echo "Old stacks removed")
+
+    echo "Sampling the task '$taskname'..."
+
+    for x in $(seq 1 $nsamples)
+    do
+        if [[ "$taskname" = "" ]]
+        then
+            arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" -ex bt \
+                2> $gdberrfile \
+                | sed -n 's/\(#.*\)/\1/p' \
+                >> $stacksfile
+        else
+            arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" \
+                                           -ex "source $root/Debug/Nuttx.py" \
+                                           -ex "show mybt $taskname" \
+                2> $gdberrfile \
+                | sed -n 's/0\.0:\(#.*\)/\1/p' \
+                >> $stacksfile
+        fi
+        echo -e '\n\n' >> $stacksfile
+        echo -ne "\r$x/$nsamples"
+        sleep $sleeptime
+    done
+
+    echo
+    echo "Stacks saved to $stacksfile"
+else
+    echo "Sampling skipped - set 'nsamples' to re-sample."
+fi
+
+#
+# Folding the stacks.
+#
+[ -f $stacksfile ] || die "Where are the stack samples?"
+
+cat << 'EOF' > /tmp/pmpn-folder.py
+#
+# This stack folder correctly handles C++ types.
+#
+from __future__ import print_function, division
+import fileinput, collections, os, sys
+
+def enforce(x, msg='Invalid input'):
+    if not x:
+        raise Exception(msg)
+
+def split_first_part_with_parens(line):
+    LBRACES = {'(':'()', '<':'<>', '[':'[]', '{':'{}'}
+    RBRACES = {')':'()', '>':'<>', ']':'[]', '}':'{}'}
+    QUOTES = set(['"', "'"])
+    quotes = collections.defaultdict(bool)
+    braces = collections.defaultdict(int)
+    out = ''
+    for ch in line:
+        out += ch
+        # escape character cancels further processing
+        if ch == '\\':
+            continue
+        # special cases
+        if out.endswith('operator>') or out.endswith('operator>>') or out.endswith('operator->'):  # gotta love c++
+            braces['<>'] += 1
+        if out.endswith('operator<') or out.endswith('operator<<'):
+            braces['<>'] -= 1
+        # switching quotes
+        if ch in QUOTES:
+            quotes[ch] = not quotes[ch]
+        # counting parens only when outside quotes
+        if sum(quotes.values()) == 0:
+            if ch in LBRACES.keys():
+                braces[LBRACES[ch]] += 1
+            if ch in RBRACES.keys():
+                braces[RBRACES[ch]] -= 1
+        # sanity check
+        for v in braces.values():
+            enforce(v >= 0, 'Unaligned braces: ' + str(dict(braces)))
+        # termination condition
+        if ch == ' ' and sum(braces.values()) == 0:
+            break
+    out = out.strip()
+    return out, line[len(out):]
+
+def parse(line):
+    def take_path(line, output):
+        line = line.strip()
+        if line.startswith('at '):
+            line = line[3:].strip()
+        if line:
+            output['file_full_path'] = line.rsplit(':', 1)[0].strip()
+            output['file_base_name'] = os.path.basename(output['file_full_path'])
+            output['line'] = int(line.rsplit(':', 1)[1])
+        return output
+
+    def take_args(line, output):
+        line = line.lstrip()
+        if line[0] == '(':
+            output['args'], line = split_first_part_with_parens(line)
+        return take_path(line.lstrip(), output)
+
+    def take_function(line, output):
+        output['function'], line = split_first_part_with_parens(line.lstrip())
+        return take_args(line.lstrip(), output)
+
+    def take_mem_loc(line, output):
+        line = line.lstrip()
+        if line.startswith('0x'):
+            end = line.find(' ')
+            num = line[:end]
+            output['memloc'] = int(num, 16)
+            line = line[end:].lstrip()
+            end = line.find(' ')
+            enforce(line[:end] == 'in')
+            line = line[end:].lstrip()
+        return take_function(line, output)
+
+    def take_frame_num(line, output):
+        line = line.lstrip()
+        enforce(line[0] == '#')
+        end = line.find(' ')
+        num = line[1:end]
+        output['frame_num'] = int(num)
+        return take_mem_loc(line[end:], output)
+
+    return take_frame_num(line, {})
+
+stacks = collections.defaultdict(int)
+current = ''
+
+stack_tops = collections.defaultdict(int)
+num_stack_frames = 0
+
+for idx,line in enumerate(fileinput.input()):
+    try:
+        line = line.strip()
+        if line:
+            inf = parse(line)
+            fun = inf['function']
+            current = (fun + ';' + current) if current else fun
+
+            if inf['frame_num'] == 0:
+                num_stack_frames += 1
+                stack_tops[fun] += 1
+        elif current:
+            stacks[current] += 1
+            current = ''
+    except Exception, ex:
+        print('ERROR (line %d):' % (idx + 1), ex, file=sys.stderr)
+
+for s, f in sorted(stacks.items(), key=lambda (s, f): s):
+    print(s, f)
+
+print('Total stack frames:', num_stack_frames, file=sys.stderr)
+print('Top consumers (distribution of the stack tops):', file=sys.stderr)
+for name,num in sorted(stack_tops.items(), key=lambda (name, num): num, reverse=True)[:10]:
+    print('% 5.1f%%   ' % (100 * num / num_stack_frames), name, file=sys.stderr)
+EOF
+
+cat $stacksfile | python /tmp/pmpn-folder.py > $foldfile
+
+echo "Folded stacks saved to $foldfile"
+
+#
+# Graphing.
+#
+cat $foldfile | flamegraph.pl --fontsize=$fgfontsize --width=$fgwidth > $graphfile
+echo "FlameGraph saved to $graphfile"
+
+# On KDE, xdg-open prefers Gwenview by default, which doesn't handle interactive SVGs, so we need a browser.
+# The current implementation is hackish and stupid. Somebody, please do something about it.
+opener=xdg-open
+which firefox       > /dev/null && opener=firefox
+which google-chrome > /dev/null && opener=google-chrome
+
+$opener $graphfile
diff --git a/Documentation/px4_block_diagram.odg b/Documentation/px4_block_diagram.odg
new file mode 100644
index 000000000..e66e1b1cd
--- /dev/null
+++ b/Documentation/px4_block_diagram.odg
diff --git a/Makefile b/Makefile
index c727c9a69..af93504ee 100644
--- a/Makefile
+++ b/Makefile
@@ -169,16 +169,18 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),)
 .NOTPARALLEL:
 endif
 
+J?=1
+
 $(ARCHIVE_DIR)%.export:	board = $(notdir $(basename $@))
 $(ARCHIVE_DIR)%.export:	configuration = nsh
 $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC)
 	@$(ECHO) %% Configuring NuttX for $(board)
 	$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
-	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean
 	$(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .)
 	$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration))
 	@$(ECHO) %% Exporting NuttX for $(board)
-	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export
+	$(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export
 	$(Q) $(MKDIR) -p $(dir $@)
 	$(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@
 	$(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board))
@@ -195,11 +197,11 @@ BOARD			 = $(BOARDS)
 menuconfig: $(NUTTX_SRC)
 	@$(ECHO) %% Configuring NuttX for $(BOARD)
 	$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
-	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean
 	$(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .)
 	$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh)
 	@$(ECHO) %% Running menuconfig for $(BOARD)
-	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig
+	$(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig
 	@$(ECHO) %% Saving configuration file
 	$(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig
 else
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 2c9387ff0..4d337171a 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -6,6 +6,11 @@
 #
 
 #
+# Start CDC/ACM serial driver
+#
+sercon
+
+#
 # Default to auto-start mode.
 #
 set MODE autostart
@@ -43,29 +48,8 @@ else
 fi
 unset FRC
 
-# if this is an APM build then there will be a rc.APM script
-# from an EXTERNAL_SCRIPTS build option
-if [ -f /etc/init.d/rc.APM ]
-then
-	if sercon
-	then
-		echo "[i] USB interface connected"
-	fi
-
-	echo "[i] Running rc.APM"
-	# if APM startup is successful then nsh will exit
-	sh /etc/init.d/rc.APM
-fi
-
 if [ $MODE == autostart ]
 then
-	echo "[i] AUTOSTART mode"
-
-	#
-	# Start CDC/ACM serial driver
-	#
-	sercon
-
 	# Try to get an USB console
 	nshterm /dev/ttyACM0 &
 
diff --git a/Tools/make_color.sh b/Tools/make_color.sh
new file mode 100755
index 000000000..81316a932
--- /dev/null
+++ b/Tools/make_color.sh
@@ -0,0 +1,36 @@
+#!/bin/sh
+# make_color.sh
+# 
+#   Author: Simon Wilks (simon@uaventure.com)
+#
+# A compiler color coder.
+#
+# To invoke this script everytime you run make simply create the alias:
+#
+#     alias make='<your-firmware-directory>/Tools/make_color.sh'
+#
+# Color codes:
+#
+# white        "\033[1,37m"
+# yellow       "\033[1,33m"
+# green        "\033[1,32m"
+# blue         "\033[1,34m"
+# cyan         "\033[1,36m"
+# red          "\033[1,31m"
+# magenta      "\033[1,35m"
+# black        "\033[1,30m"
+# darkwhite    "\033[0,37m"
+# darkyellow   "\033[0,33m"
+# darkgreen    "\033[0,32m"
+# darkblue     "\033[0,34m"
+# darkcyan     "\033[0,36m"
+# darkred      "\033[0,31m"
+# darkmagenta  "\033[0,35m"
+# off          "\033[0,0m"
+#
+OFF="\o033[0m"
+WARN="\o033[1;33m"
+ERROR="\o033[1;31m" 
+INFO="\o033[0;37m"
+
+make ${@} 2>&1 | sed "s/make\[[0-9]\].*/$INFO & $OFF/;s/.*: warning: .*/$WARN & $OFF/;s/.*: error: .*/$ERROR & $OFF/" 
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 7eb090d73..4e528f8cd 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -62,6 +62,7 @@ MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
+MODULES 	+= modules/land_detector
 
 #
 # Estimation modules (EKF / other filters)
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 2ce738844..5d6beffd7 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -37,11 +37,11 @@ MODULES		+= drivers/gps
 # MODULES		+= drivers/hott/hott_sensors
 # MODULES		+= drivers/blinkm
 MODULES		+= drivers/airspeed
-MODULES		+= drivers/ets_airspeed
+# MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
-MODULES		+= drivers/frsky_telemetry
+# MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors
-MODULES		+= drivers/mkblctrl
+# MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/px4flow
 
 #
@@ -70,7 +70,7 @@ MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
-MODULES		+= modules/uavcan
+# MODULES		+= modules/uavcan
 MODULES 	+= modules/land_detector
 
 #
@@ -121,7 +121,7 @@ MODULES		+= platforms/nuttx
 #
 # OBC challenge
 #
-MODULES		+= modules/bottle_drop
+# MODULES		+= modules/bottle_drop
 
 #
 # Demo apps
diff --git a/makefiles/setup.mk b/makefiles/setup.mk
index 435c240bf..351b9f1b7 100644
--- a/makefiles/setup.mk
+++ b/makefiles/setup.mk
@@ -82,6 +82,7 @@ export ECHO		 = echo
 export UNZIP_CMD	 = unzip
 export PYTHON		 = python
 export OPENOCD		 = openocd
+export GREP		 = grep
 
 #
 # Host-specific paths, hacks and fixups
diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index 396980453..dc8d3f712 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -80,13 +80,20 @@ ARCHCPUFLAGS_CORTEXM3	 = -mcpu=cortex-m3 \
 			   -march=armv7-m \
 			   -mfloat-abi=soft
 
-ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \
-			   -ffixed-r10
-
-ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \
-			   -ffixed-r10
-
-ARCHINSTRUMENTATIONDEFINES_CORTEXM3 =
+# Enabling stack checks if OS was build with them
+#
+TEST_FILE_STACKCHECK=$(WORK_DIR)nuttx-export/include/nuttx/config.h
+TEST_VALUE_STACKCHECK=CONFIG_ARMV7M_STACKCHECK\ 1
+ENABLE_STACK_CHECKS=$(shell $(GREP) -q "$(TEST_VALUE_STACKCHECK)" $(TEST_FILE_STACKCHECK); echo $$?;)
+ifeq ("$(ENABLE_STACK_CHECKS)","0")
+ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10
+ARCHINSTRUMENTATIONDEFINES_CORTEXM4  = -finstrument-functions -ffixed-r10
+ARCHINSTRUMENTATIONDEFINES_CORTEXM3  =
+else
+ARCHINSTRUMENTATIONDEFINES_CORTEXM4F =
+ARCHINSTRUMENTATIONDEFINES_CORTEXM4  =
+ARCHINSTRUMENTATIONDEFINES_CORTEXM3  =
+endif
 
 # Pick the right set of flags for the architecture.
 #
@@ -105,7 +112,7 @@ ARCHDEFINES		+= -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD)
 # optimisation flags
 #
 ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
-			   -g \
+			   -g3 \
 			   -fno-strict-aliasing \
 			   -fno-strength-reduce \
 			   -fomit-frame-pointer \
diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs
index c1f5a8ac4..3808fc1cf 100644
--- a/nuttx-configs/aerocore/nsh/Make.defs
+++ b/nuttx-configs/aerocore/nsh/Make.defs
@@ -62,8 +62,9 @@ ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
 
 
 # enable precise stack overflow tracking
-INSTRUMENTATIONDEFINES	 = -finstrument-functions \
-			   -ffixed-r10
+ifeq ($(CONFIG_ARMV7M_STACKCHECK),y)
+INSTRUMENTATIONDEFINES	 = -finstrument-functions -ffixed-r10
+endif
 
 # pull in *just* libm from the toolchain ... this is grody
 LIBM			 = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig
index 317194f68..c44b074f3 100644
--- a/nuttx-configs/aerocore/nsh/defconfig
+++ b/nuttx-configs/aerocore/nsh/defconfig
@@ -94,7 +94,7 @@ CONFIG_ARCH_HAVE_MPU=y
 # CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set
 # CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set
 CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
-CONFIG_ARMV7M_STACKCHECK=y
+CONFIG_ARMV7M_STACKCHECK=n
 CONFIG_SERIAL_TERMIOS=y
 
 #
diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs
index 5e28f2473..4e08d28a2 100644
--- a/nuttx-configs/px4fmu-v1/nsh/Make.defs
+++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs
@@ -62,8 +62,9 @@ ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
 
 
 # enable precise stack overflow tracking
-INSTRUMENTATIONDEFINES	 = -finstrument-functions \
-			   -ffixed-r10
+ifeq ($(CONFIG_ARMV7M_STACKCHECK),y)
+INSTRUMENTATIONDEFINES	 = -finstrument-functions -ffixed-r10
+endif
 
 # pull in *just* libm from the toolchain ... this is grody
 LIBM			 = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs
index 99f3b3140..5a1d5af2c 100644
--- a/nuttx-configs/px4fmu-v2/nsh/Make.defs
+++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs
@@ -62,8 +62,9 @@ ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
 
 
 # enable precise stack overflow tracking
-INSTRUMENTATIONDEFINES	 = -finstrument-functions \
-			   -ffixed-r10
+ifeq ($(CONFIG_ARMV7M_STACKCHECK),y)
+INSTRUMENTATIONDEFINES	 = -finstrument-functions -ffixed-r10
+endif
 
 # pull in *just* libm from the toolchain ... this is grody
 LIBM			 = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs
index b4f5577ae..74f38c0cb 100644
--- a/nuttx-configs/px4io-v1/nsh/Make.defs
+++ b/nuttx-configs/px4io-v1/nsh/Make.defs
@@ -58,6 +58,11 @@ ARCHCPUFLAGS		 = -mcpu=cortex-m3 \
 			   -mthumb \
 			   -march=armv7-m
 
+# enable precise stack overflow tracking
+ifeq ($(CONFIG_ARMV7M_STACKCHECK),y)
+INSTRUMENTATIONDEFINES	 = -finstrument-functions -ffixed-r10
+endif
+
 # use our linker script
 LDSCRIPT		 = ld.script
 
diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs
index 51420eb23..287466db6 100644
--- a/nuttx-configs/px4io-v2/nsh/Make.defs
+++ b/nuttx-configs/px4io-v2/nsh/Make.defs
@@ -58,6 +58,11 @@ ARCHCPUFLAGS		 = -mcpu=cortex-m3 \
 			   -mthumb \
 			   -march=armv7-m
 
+# enable precise stack overflow tracking
+ifeq ($(CONFIG_ARMV7M_STACKCHECK),y)
+INSTRUMENTATIONDEFINES	 = -finstrument-functions -ffixed-r10
+endif
+
 # use our linker script
 LDSCRIPT		 = ld.script
 
diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp
index 2b5fef4d7..92b752a28 100644
--- a/src/drivers/batt_smbus/batt_smbus.cpp
+++ b/src/drivers/batt_smbus/batt_smbus.cpp
@@ -91,6 +91,7 @@
 #define BATT_SMBUS_MANUFACTURE_INFO		0x25	///< cell voltage register
 #define BATT_SMBUS_CURRENT			0x2a	///< current register
 #define BATT_SMBUS_MEASUREMENT_INTERVAL_MS	(1000000 / 10)	///< time in microseconds, measure at 10hz
+#define BATT_SMBUS_TIMEOUT_MS		10000000	///< timeout looking for battery 10seconds after startup
 
 #define BATT_SMBUS_PEC_POLYNOMIAL		0x07	///< Polynomial for calculating PEC
 
@@ -171,11 +172,13 @@ private:
 	uint8_t			get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const;
 
 	// internal variables
+	bool			_enabled;	///< true if we have successfully connected to battery
 	work_s			_work;		///< work queue for scheduling reads
 	RingBuffer		*_reports;	///< buffer of recorded voltages, currents
 	struct battery_status_s _last_report;	///< last published report, used for test()
 	orb_advert_t		_batt_topic;	///< uORB battery topic
 	orb_id_t		_batt_orb_id;	///< uORB battery topic ID
+	uint64_t		_start_time;	///< system time we first attempt to communicate with battery
 };
 
 namespace
@@ -189,13 +192,18 @@ extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
 
 BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) :
 	I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000),
+	_enabled(false),
 	_work{},
 	_reports(nullptr),
 	_batt_topic(-1),
-	_batt_orb_id(nullptr)
+	_batt_orb_id(nullptr),
+	_start_time(0)
 {
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
+
+	// capture startup time
+	_start_time = hrt_absolute_time();
 }
 
 BATT_SMBUS::~BATT_SMBUS()
@@ -330,11 +338,20 @@ BATT_SMBUS::cycle_trampoline(void *arg)
 void
 BATT_SMBUS::cycle()
 {
+	// get current time
+	uint64_t now = hrt_absolute_time();
+
+	// exit without rescheduling if we have failed to find a battery after 10 seconds
+	if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) {
+		warnx("did not find smart battery");
+		return;
+	}
+
 	// read data from sensor
 	struct battery_status_s new_report;
 
 	// set time of reading
-	new_report.timestamp = hrt_absolute_time();
+	new_report.timestamp = now;
 
 	// read voltage
 	uint16_t tmp;
@@ -375,6 +392,9 @@ BATT_SMBUS::cycle()
 
 		// notify anyone waiting for data
 		poll_notify(POLLIN);
+
+		// record we are working
+		_enabled = true;
 	}
 
 	// schedule a fresh cycle call when the measurement is done
diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h
index 0f8362f58..12d51aeaa 100644
--- a/src/drivers/drv_range_finder.h
+++ b/src/drivers/drv_range_finder.h
@@ -45,6 +45,7 @@
 #include "drv_orb_dev.h"
 
 #define RANGE_FINDER_DEVICE_PATH	"/dev/range_finder"
+#define MB12XX_MAX_RANGEFINDERS	12	//Maximum number of RangeFinders that can be connected
 
 enum RANGE_FINDER_TYPE {
 	RANGE_FINDER_TYPE_LASER = 0,
@@ -67,6 +68,8 @@ struct range_finder_report {
 	float minimum_distance;			/**< minimum distance the sensor can measure */
 	float maximum_distance;			/**< maximum distance the sensor can measure */
 	uint8_t valid;				/**< 1 == within sensor range, 0 = outside sensor range */
+	float distance_vector[MB12XX_MAX_RANGEFINDERS]; /** in meters */
+	uint8_t just_updated;			/** number of the most recent measurement sensor */
 };
 
 /**
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 95fbed0ba..a06be72d9 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1351,13 +1351,15 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
 	}
 			
 	int fd = open(bus.devpath, O_RDONLY);
-	if (fd < 0)
+	if (fd < 0) {
 		return false;
+	}
 
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		close(fd);
 		errx(1,"Failed to setup poll rate");
 	}
+	close(fd);
 
 	return true;
 }
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 9cf568658..a4fb7bcc2 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 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
@@ -34,6 +34,7 @@
 /**
  * @file mb12xx.cpp
  * @author Greg Hulands
+ * @author Jon Verbeke <jon.verbeke@kuleuven.be>
  *
  * Driver for the Maxbotix sonar range finders connected via I2C.
  */
@@ -54,6 +55,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <vector>
 
 #include <nuttx/arch.h>
 #include <nuttx/wqueue.h>
@@ -72,7 +74,7 @@
 #include <board_config.h>
 
 /* Configuration Constants */
-#define MB12XX_BUS 			PX4_I2C_BUS_EXPANSION
+#define MB12XX_BUS 		PX4_I2C_BUS_EXPANSION
 #define MB12XX_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
 #define MB12XX_DEVICE_PATH	"/dev/mb12xx"
 
@@ -83,10 +85,12 @@
 #define MB12XX_SET_ADDRESS_2	0xA5		/* Change address 2 Register */
 
 /* Device limits */
-#define MB12XX_MIN_DISTANCE (0.20f)
-#define MB12XX_MAX_DISTANCE (7.65f)
+#define MB12XX_MIN_DISTANCE 	(0.20f)
+#define MB12XX_MAX_DISTANCE 	(7.65f)
+
+#define MB12XX_CONVERSION_INTERVAL 	100000 /* 60ms for one sonar */
+#define TICKS_BETWEEN_SUCCESIVE_FIRES 	100000 /* 30ms between each sonar measurement (watch out for interference!) */
 
-#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -133,12 +137,19 @@ private:
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
 
+	uint8_t				_cycle_counter;	/* counter in cycle to change i2c adresses */
+	int					_cycling_rate;	/* */
+	uint8_t				_index_counter;	/* temporary sonar i2c address */
+	std::vector<uint8_t>	addr_ind; 	/* temp sonar i2c address vector */
+	std::vector<float>	_latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */
+
+
 	/**
 	* Test whether the device supported by the driver is present at a
 	* specific address.
 	*
 	* @param address	The I2C bus address to probe.
-	* @return		True if the device is present.
+	* @return			True if the device is present.
 	*/
 	int					probe_address(uint8_t address);
 
@@ -178,7 +189,7 @@ private:
 	*
 	* @param arg		Instance pointer for the driver that is polling.
 	*/
-	static void		cycle_trampoline(void *arg);
+	static void			cycle_trampoline(void *arg);
 
 
 };
@@ -200,12 +211,16 @@ MB12XX::MB12XX(int bus, int address) :
 	_range_finder_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
+	_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")),
+	_cycle_counter(0),	/* initialising counter for cycling function to zero */
+	_cycling_rate(0),	/* initialising cycling rate (which can differ depending on one sonar or multiple) */
+	_index_counter(0) 	/* initialising temp sonar i2c address to zero */
+
 {
-	// enable debug() calls
+	/* enable debug() calls */
 	_debug_enabled = false;
 
-	// work_cancel in the dtor will explode if we don't do this...
+	/* work_cancel in the dtor will explode if we don't do this... */
 	memset(&_work, 0, sizeof(_work));
 }
 
@@ -223,7 +238,7 @@ MB12XX::~MB12XX()
 		unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
 	}
 
-	// free perf counters
+	/* free perf counters */
 	perf_free(_sample_perf);
 	perf_free(_comms_errors);
 	perf_free(_buffer_overflows);
@@ -242,6 +257,9 @@ MB12XX::init()
 	/* allocate basic report buffers */
 	_reports = new RingBuffer(2, sizeof(range_finder_report));
 
+	_index_counter = MB12XX_BASEADDR;	/* set temp sonar i2c address to base adress */
+	set_address(_index_counter);		/* set I2c port to temp sonar i2c adress */
+
 	if (_reports == nullptr) {
 		goto out;
 	}
@@ -250,16 +268,51 @@ MB12XX::init()
 
 	if (_class_instance == CLASS_DEVICE_PRIMARY) {
 		/* get a publish handle on the range finder topic */
-		struct range_finder_report rf_report;
-		measure();
-		_reports->get(&rf_report);
+		struct range_finder_report rf_report = {};
+
 		_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
 
 		if (_range_finder_topic < 0) {
-			debug("failed to create sensor_range_finder object. Did you start uOrb?");
+			log("failed to create sensor_range_finder object. Did you start uOrb?");
+		}
+	}
+
+	// XXX we should find out why we need to wait 200 ms here
+	usleep(200000);
+
+	/* check for connected rangefinders on each i2c port:
+	   We start from i2c base address (0x70 = 112) and count downwards
+	   So second iteration it uses i2c address 111, third iteration 110 and so on*/
+	for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) {
+		_index_counter = MB12XX_BASEADDR - counter;	/* set temp sonar i2c address to base adress - counter */
+		set_address(_index_counter);			/* set I2c port to temp sonar i2c adress */
+		int ret2 = measure();
+
+		if (ret2 == 0) { /* sonar is present -> store address_index in array */
+			addr_ind.push_back(_index_counter);
+			debug("sonar added");
+			_latest_sonar_measurements.push_back(200);
 		}
 	}
 
+	_index_counter = MB12XX_BASEADDR;
+	set_address(_index_counter); /* set i2c port back to base adress for rest of driver */
+
+	/* if only one sonar detected, no special timing is required between firing, so use default */
+	if (addr_ind.size() == 1) {
+		_cycling_rate = MB12XX_CONVERSION_INTERVAL;
+
+	} else {
+		_cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES;
+	}
+
+	/* show the connected sonars in terminal */
+	for (unsigned i = 0; i < addr_ind.size(); i++) {
+		log("sonar %d with address %d added", (i + 1), addr_ind[i]);
+	}
+
+	debug("Number of sonars connected: %d", addr_ind.size());
+
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
 	_sensor_ok = true;
@@ -325,11 +378,12 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 					bool want_start = (_measure_ticks == 0);
 
 					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL);
+					_measure_ticks = USEC2TICK(_cycling_rate);
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start) {
 						start();
+
 					}
 
 					return OK;
@@ -341,10 +395,10 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 					bool want_start = (_measure_ticks == 0);
 
 					/* convert hz to tick interval via microseconds */
-					unsigned ticks = USEC2TICK(1000000 / arg);
+					int ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
+					if (ticks < USEC2TICK(_cycling_rate)) {
 						return -EINVAL;
 					}
 
@@ -414,6 +468,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 ssize_t
 MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 {
+
 	unsigned count = buflen / sizeof(struct range_finder_report);
 	struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
 	int ret = 0;
@@ -453,7 +508,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* wait for it to complete */
-		usleep(MB12XX_CONVERSION_INTERVAL);
+		usleep(_cycling_rate * 2);
 
 		/* run the collection phase */
 		if (OK != collect()) {
@@ -474,17 +529,19 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 int
 MB12XX::measure()
 {
+
 	int ret;
 
 	/*
 	 * Send the command to begin a measurement.
 	 */
+
 	uint8_t cmd = MB12XX_TAKE_RANGE_REG;
 	ret = transfer(&cmd, 1, nullptr, 0);
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		log("i2c::transfer returned %d", ret);
+		debug("i2c::transfer returned %d", ret);
 		return ret;
 	}
 
@@ -506,7 +563,7 @@ MB12XX::collect()
 	ret = transfer(nullptr, 0, &val[0], 2);
 
 	if (ret < 0) {
-		log("error reading from sensor: %d", ret);
+		debug("error reading from sensor: %d", ret);
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 		return ret;
@@ -519,7 +576,39 @@ MB12XX::collect()
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
 	report.error_count = perf_event_count(_comms_errors);
-	report.distance = si_units;
+
+	/* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */
+	if (addr_ind.size() == 1) {
+		report.distance = si_units;
+
+		for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) {
+			report.distance_vector[i] = 0;
+		}
+
+		report.just_updated = 0;
+
+	} else {
+		/* for multiple sonars connected */
+
+		/* don't use the orginial single sonar variable */
+		report.distance = 0;
+
+		/* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values 			of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest 			value for each connected sonar */
+		_latest_sonar_measurements[_cycle_counter] = si_units;
+
+		for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) {
+			report.distance_vector[i] = _latest_sonar_measurements[i];
+		}
+
+		/* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been 		collected as this could be of use for Kalman filters */
+		report.just_updated = _index_counter;
+
+		/* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */
+		for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) {
+			report.distance_vector[addr_ind.size() + i] = 0;
+		}
+	}
+
 	report.minimum_distance = get_minimum_distance();
 	report.maximum_distance = get_maximum_distance();
 	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
@@ -545,12 +634,13 @@ MB12XX::collect()
 void
 MB12XX::start()
 {
+
 	/* reset the report ring and state machine */
 	_collect_phase = false;
 	_reports->flush();
 
 	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
+	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5);
 
 	/* notify about state change */
 	struct subsystem_info_s info = {
@@ -564,8 +654,10 @@ MB12XX::start()
 	if (pub > 0) {
 		orb_publish(ORB_ID(subsystem_info), pub, &info);
 
+
 	} else {
 		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+
 	}
 }
 
@@ -578,21 +670,24 @@ MB12XX::stop()
 void
 MB12XX::cycle_trampoline(void *arg)
 {
+
 	MB12XX *dev = (MB12XX *)arg;
 
 	dev->cycle();
+
 }
 
 void
 MB12XX::cycle()
 {
-	/* collection phase? */
 	if (_collect_phase) {
+		_index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */
+		set_address(_index_counter);
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
-			/* restart the measurement state machine */
+			debug("collection error");
+			/* if error restart the measurement state machine */
 			start();
 			return;
 		}
@@ -600,25 +695,37 @@ MB12XX::cycle()
 		/* next phase is measurement */
 		_collect_phase = false;
 
-		/*
-		 * Is there a collect->measure gap?
-		 */
-		if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
+		/* change i2c adress to next sonar */
+		_cycle_counter = _cycle_counter + 1;
+
+		if (_cycle_counter >= addr_ind.size()) {
+			_cycle_counter = 0;
+		}
+
+		/* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate
+		   Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */
+
+		if (_measure_ticks > USEC2TICK(_cycling_rate)) {
 
 			/* schedule a fresh cycle call when we are ready to measure again */
 			work_queue(HPWORK,
 				   &_work,
 				   (worker_t)&MB12XX::cycle_trampoline,
 				   this,
-				   _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL));
-
+				   _measure_ticks - USEC2TICK(_cycling_rate));
 			return;
 		}
 	}
 
-	/* measurement phase */
+	/* Measurement (firing) phase */
+
+	/* ensure sonar i2c adress is still correct */
+	_index_counter = addr_ind[_cycle_counter];
+	set_address(_index_counter);
+
+	/* Perform measurement */
 	if (OK != measure()) {
-		log("measure error");
+		debug("measure error sonar adress %d", _index_counter);
 	}
 
 	/* next phase is collection */
@@ -629,7 +736,8 @@ MB12XX::cycle()
 		   &_work,
 		   (worker_t)&MB12XX::cycle_trampoline,
 		   this,
-		   USEC2TICK(MB12XX_CONVERSION_INTERVAL));
+		   USEC2TICK(_cycling_rate));
+
 }
 
 void
@@ -750,7 +858,7 @@ test()
 	}
 
 	warnx("single read");
-	warnx("measurement: %0.2f m", (double)report.distance);
+	warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated);
 	warnx("time:        %lld", report.timestamp);
 
 	/* start the sensor polling at 2Hz */
@@ -779,7 +887,12 @@ test()
 		}
 
 		warnx("periodic read %u", i);
-		warnx("measurement: %0.3f", (double)report.distance);
+
+		/* Print the sonar rangefinder report sonar distance vector */
+		for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) {
+			warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1);
+		}
+
 		warnx("time:        %lld", report.timestamp);
 	}
 
@@ -830,7 +943,7 @@ info()
 	exit(0);
 }
 
-} // namespace
+} /* namespace */
 
 int
 mb12xx_main(int argc, char *argv[])
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 168b34ea9..2be758244 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -921,26 +921,50 @@ MPU6000::gyro_self_test()
 	if (self_test())
 		return 1;
 
-	/* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
-	if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
+	/* 
+	 * Maximum deviation of 20 degrees, according to
+	 * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf
+	 * Section 6.1, initial ZRO tolerance
+	 */
+	const float max_offset = 0.34f;
+	/* 30% scale error is chosen to catch completely faulty units but
+	 * to let some slight scale error pass. Requires a rate table or correlation
+	 * with mag rotations + data fit to
+	 * calibrate properly and is not done by default.
+	 */
+	const float max_scale = 0.3f;
+
+	/* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */
+	if (fabsf(_gyro_scale.x_offset) > max_offset)
 		return 1;
-	if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
+
+	/* evaluate gyro scale, complain if off by more than 30% */
+	if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale)
 		return 1;
 
-	if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
+	if (fabsf(_gyro_scale.y_offset) > max_offset)
 		return 1;
-	if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
+	if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale)
 		return 1;
 
-	if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
+	if (fabsf(_gyro_scale.z_offset) > max_offset)
 		return 1;
-	if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
+	if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale)
 		return 1;
 
+	/* check if all scales are zero */
+	if ((fabsf(_gyro_scale.x_offset) < 0.000001f) &&
+		(fabsf(_gyro_scale.y_offset) < 0.000001f) &&
+		(fabsf(_gyro_scale.z_offset) < 0.000001f)) {
+		/* if all are zero, this device is not calibrated */
+		return 1;
+	}
+
 	return 0;
 }
 
 
+
 /*
   perform a self-test comparison to factory trim values. This takes
   about 200ms and will return OK if the current values are within 14%
diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h
new file mode 100644
index 000000000..b391b1f6a
--- /dev/null
+++ b/src/drivers/px4flow/i2c_frame.h
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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 i2c_frame.h
+ * Definition of i2c frames.
+ * @author Thomas Boehm <thomas.boehm@fortiss.org>
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#ifndef I2C_FRAME_H_
+#define I2C_FRAME_H_
+#include <inttypes.h>
+
+
+typedef  struct i2c_frame
+{
+    uint16_t frame_count;
+    int16_t pixel_flow_x_sum;
+    int16_t pixel_flow_y_sum;
+    int16_t flow_comp_m_x;
+    int16_t flow_comp_m_y;
+    int16_t qual;
+    int16_t gyro_x_rate;
+    int16_t gyro_y_rate;
+    int16_t gyro_z_rate;
+    uint8_t gyro_range;
+    uint8_t sonar_timestamp;
+    int16_t ground_distance;
+} i2c_frame;
+
+#define I2C_FRAME_SIZE (sizeof(i2c_frame))
+
+
+typedef struct i2c_integral_frame
+{
+    uint16_t frame_count_since_last_readout;
+    int16_t pixel_flow_x_integral;
+    int16_t pixel_flow_y_integral;
+    int16_t gyro_x_rate_integral;
+    int16_t gyro_y_rate_integral;
+    int16_t gyro_z_rate_integral;
+    uint32_t integration_timespan;
+    uint32_t sonar_timestamp;
+    uint16_t ground_distance;
+    int16_t gyro_temperature;
+    uint8_t qual;
+} i2c_integral_frame;
+
+#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame))
+
+#endif /* I2C_FRAME_H_ */
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 9c9c1b0f8..bb0cdbbb6 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -93,38 +93,11 @@ static const int ERROR = -1;
 # error This requires CONFIG_SCHED_WORKQUEUE.
 #endif
 
-struct i2c_frame {
-	uint16_t frame_count;
-	int16_t pixel_flow_x_sum;
-	int16_t pixel_flow_y_sum;
-	int16_t flow_comp_m_x;
-	int16_t flow_comp_m_y;
-	int16_t qual;
-	int16_t gyro_x_rate;
-	int16_t gyro_y_rate;
-	int16_t gyro_z_rate;
-	uint8_t gyro_range;
-	uint8_t sonar_timestamp;
-	int16_t ground_distance;
-};
-struct i2c_frame f;
+#include "i2c_frame.h"
 
-struct i2c_integral_frame {
-	uint16_t frame_count_since_last_readout;
-	int16_t pixel_flow_x_integral;
-	int16_t pixel_flow_y_integral;
-	int16_t gyro_x_rate_integral;
-	int16_t gyro_y_rate_integral;
-	int16_t gyro_z_rate_integral;
-	uint32_t integration_timespan;
-	uint32_t time_since_last_sonar_update;
-	uint16_t ground_distance;
-	int16_t gyro_temperature;
-	uint8_t qual;
-} __attribute__((packed));
+struct i2c_frame f;
 struct i2c_integral_frame f_integral;
 
-
 class PX4FLOW: public device::I2C
 {
 public:
@@ -150,8 +123,7 @@ private:
 	RingBuffer			*_reports;
 	bool				_sensor_ok;
 	int					_measure_ticks;
-	bool				_collect_phase;
-
+	bool				_collect_phase; 
 	orb_advert_t		_px4flow_topic;
 
 	perf_counter_t		_sample_perf;
@@ -261,10 +233,10 @@ out:
 int
 PX4FLOW::probe()
 {
-	uint8_t val[22];
+	uint8_t val[I2C_FRAME_SIZE];
 
 	// to be sure this is not a ll40ls Lidar (which can also be on
-	// 0x42) we check if a 22 byte transfer works from address
+	// 0x42) we check if a I2C_FRAME_SIZE byte transfer works from address
 	// 0. The ll40ls gives an error for that, whereas the flow
 	// happily returns some data
 	if (transfer(nullptr, 0, &val[0], 22) != OK) {
@@ -469,16 +441,16 @@ PX4FLOW::collect()
 	int ret = -EIO;
 
 	/* read from the sensor */
-	uint8_t val[47] = { 0 };
+	uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 };
 
 	perf_begin(_sample_perf);
 
 	if (PX4FLOW_REG == 0x00) {
-		ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2)
+		ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE);
 	}
 
 	if (PX4FLOW_REG == 0x16) {
-		ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2)
+		ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE);
 	}
 
 	if (ret < 0) {
@@ -489,46 +461,12 @@ PX4FLOW::collect()
 	}
 
 	if (PX4FLOW_REG == 0) {
-		f.frame_count = val[1] << 8 | val[0];
-		f.pixel_flow_x_sum = val[3] << 8 | val[2];
-		f.pixel_flow_y_sum = val[5] << 8 | val[4];
-		f.flow_comp_m_x = val[7] << 8 | val[6];
-		f.flow_comp_m_y = val[9] << 8 | val[8];
-		f.qual = val[11] << 8 | val[10];
-		f.gyro_x_rate = val[13] << 8 | val[12];
-		f.gyro_y_rate = val[15] << 8 | val[14];
-		f.gyro_z_rate = val[17] << 8 | val[16];
-		f.gyro_range = val[18];
-		f.sonar_timestamp = val[19];
-		f.ground_distance = val[21] << 8 | val[20];
-
-		f_integral.frame_count_since_last_readout = val[23] << 8 | val[22];
-		f_integral.pixel_flow_x_integral = val[25] << 8 | val[24];
-		f_integral.pixel_flow_y_integral = val[27] << 8 | val[26];
-		f_integral.gyro_x_rate_integral = val[29] << 8 | val[28];
-		f_integral.gyro_y_rate_integral = val[31] << 8 | val[30];
-		f_integral.gyro_z_rate_integral = val[33] << 8 | val[32];
-		f_integral.integration_timespan = val[37] << 24 | val[36] << 16
-						  | val[35] << 8 | val[34];
-		f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16
-				| val[39] << 8 | val[38];
-		f_integral.ground_distance = val[43] << 8 | val[42];
-		f_integral.gyro_temperature = val[45] << 8 | val[44];
-		f_integral.qual = val[46];
+		memcpy(&f, val, I2C_FRAME_SIZE);
+		memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
 	}
 
 	if (PX4FLOW_REG == 0x16) {
-		f_integral.frame_count_since_last_readout = val[1] << 8 | val[0];
-		f_integral.pixel_flow_x_integral = val[3] << 8 | val[2];
-		f_integral.pixel_flow_y_integral = val[5] << 8 | val[4];
-		f_integral.gyro_x_rate_integral = val[7] << 8 | val[6];
-		f_integral.gyro_y_rate_integral = val[9] << 8 | val[8];
-		f_integral.gyro_z_rate_integral = val[11] << 8 | val[10];
-		f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12];
-		f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16];
-		f_integral.ground_distance = val[21] << 8 | val[20];
-		f_integral.gyro_temperature = val[23] << 8 | val[22];
-		f_integral.qual = val[24];
+		memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
 	}
 
 
@@ -544,7 +482,7 @@ PX4FLOW::collect()
 	report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
 	report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
 	report.integration_timespan = f_integral.integration_timespan; //microseconds
-	report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds
+	report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds
 	report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
 
 	report.sensor_id = 0;
@@ -828,7 +766,7 @@ test()
 		warnx("ground_distance: %0.2f m",
 		      (double) f_integral.ground_distance / 1000);
 		warnx("time since last sonar update [us]: %i",
-		      f_integral.time_since_last_sonar_update);
+		      f_integral.sonar_timestamp);
 		warnx("quality integration average : %i", f_integral.qual);
 		warnx("quality : %i", f.qual);
 
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 0451bbd1b..8ce548571 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -2860,10 +2860,10 @@ checkcrc(int argc, char *argv[])
 	}
 
 	if (ret != OK) {
-		printf("check CRC failed - %d\n", ret);
+		printf("[PX4IO::checkcrc] check CRC failed - %d\n", ret);
 		exit(1);
 	}
-	printf("CRCs match\n");
+	printf("[PX4IO::checkcrc] CRCs match\n");
 	exit(0);
 }
 
diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index fb16f891f..02e527695 100644
--- a/src/drivers/px4io/px4io_uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -621,6 +621,7 @@ int
 PX4IO_Uploader::reboot()
 {
 	send(PROTO_REBOOT);
+	up_udelay(100*1000); // Ensure the farend is in wait for char.
 	send(PROTO_EOC);
 
 	return OK;
diff --git a/src/lib/uavcan b/src/lib/uavcan
-Subproject c4c45b995f5c8192c7a36c4293c201711ceac74
+Subproject 7719f7692aba67f01b6321773bb7be13f23d2f6
diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk
index 1158536e1..d158d7a49 100644
--- a/src/modules/attitude_estimator_ekf/module.mk
+++ b/src/modules/attitude_estimator_ekf/module.mk
@@ -43,6 +43,6 @@ SRCS		 = attitude_estimator_ekf_main.cpp \
 
 MODULE_STACKSIZE = 1200
 
-EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3600
+EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700
 
 EXTRACXXFLAGS = -Wframe-larger-than=2400
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index f4e3dc441..2e28a6d2c 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -435,7 +435,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 
 	/* Use distance value for range finder report */
 	struct range_finder_report r;
-	memset(&r, 0, sizeof(f));
+	memset(&r, 0, sizeof(r));
 
 	r.timestamp = hrt_absolute_time();
 	r.error_count = 0;
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 562033db1..f20bba52e 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -35,6 +35,10 @@
  * @file mc_att_control_main.cpp
  * Multicopter attitude controller.
  *
+ * Publication for the desired attitude tracking:
+ * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors.
+ * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011.
+ *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index bf65d2805..60682fb8e 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -35,6 +35,12 @@
  * @file mc_pos_control_main.cpp
  * Multicopter position controller.
  *
+ * Original publication for the desired attitude generation:
+ * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors.
+ * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011
+ *
+ * Also inspired by https://pixhawk.org/firmware/apps/fw_pos_control_l1
+ *
  * The controller has two loops: P loop for position error and PID loop for velocity error.
  * Output of velocity controller is thrust vector that splitted to thrust direction
  * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
@@ -727,11 +733,18 @@ MulticopterPositionControl::control_auto(float dt)
 		reset_alt_sp();
 	}
 
+	//Poll position setpoint
 	bool updated;
 	orb_check(_pos_sp_triplet_sub, &updated);
-
 	if (updated) {
 		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+
+		//Make sure that the position setpoint is valid
+		if (!isfinite(_pos_sp_triplet.current.lat) || 
+			!isfinite(_pos_sp_triplet.current.lon) || 
+			!isfinite(_pos_sp_triplet.current.alt)) {
+			_pos_sp_triplet.current.valid = false;
+		}
 	}
 
 	if (_pos_sp_triplet.current.valid) {
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 4482fb36b..9bc9be245 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -58,17 +58,17 @@
 static const int ERROR = -1;
 
 Geofence::Geofence() :
-		SuperBlock(NULL, "GF"),
-		_fence_pub(-1),
-		_altitude_min(0),
-		_altitude_max(0),
-		_verticesCount(0),
-		_param_geofence_on(this, "ON"),
-		_param_altitude_mode(this, "ALTMODE"),
-		_param_source(this, "SOURCE"),
-		_param_counter_threshold(this, "COUNT"),
-		_outside_counter(0),
-		_mavlinkFd(-1)
+	SuperBlock(NULL, "GF"),
+	_fence_pub(-1),
+	_altitude_min(0),
+	_altitude_max(0),
+	_verticesCount(0),
+	_param_geofence_on(this, "ON"),
+	_param_altitude_mode(this, "ALTMODE"),
+	_param_source(this, "SOURCE"),
+	_param_counter_threshold(this, "COUNT"),
+	_outside_counter(0),
+	_mavlinkFd(-1)
 {
 	/* Load initial params */
 	updateParams();
@@ -92,22 +92,26 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f
 
 
 bool Geofence::inside(const struct vehicle_global_position_s &global_position,
-			const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) {
+		      const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl)
+{
 	updateParams();
 
 	if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
 		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
 			return inside(global_position);
+
 		} else {
 			return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
-					(double)gps_position.alt * 1.0e-3);
+				      (double)gps_position.alt * 1.0e-3);
 		}
+
 	} else {
 		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
 			return inside(global_position, baro_altitude_amsl);
+
 		} else {
 			return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
-					baro_altitude_amsl);
+				      baro_altitude_amsl);
 		}
 	}
 }
@@ -120,9 +124,12 @@ bool Geofence::inside(double lat, double lon, float altitude)
 		_outside_counter = 0;
 		return inside_fence;
 	} {
+
 		_outside_counter++;
-		if(_outside_counter > _param_counter_threshold.get()) {
+
+		if (_outside_counter > _param_counter_threshold.get()) {
 			return inside_fence;
+
 		} else {
 			return true;
 		}
@@ -133,8 +140,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
 bool Geofence::inside_polygon(double lat, double lon, float altitude)
 {
 	/* Return true if geofence is disabled */
-	if (_param_geofence_on.get() != 1)
+	if (_param_geofence_on.get() != 1) {
 		return true;
+	}
 
 	if (valid()) {
 
@@ -159,20 +167,22 @@ bool Geofence::inside_polygon(double lat, double lon, float altitude)
 				if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
 					break;
 				}
+
 				if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
 					break;
 				}
 
 				// skip vertex 0 (return point)
 				if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) &&
-							(lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
-							 (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
-							c = !c;
+				    (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
+				     (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
+					c = !c;
 				}
 
 			}
 
 			return c;
+
 		} else {
 			/* Empty fence --> accept all points */
 			return true;
@@ -188,8 +198,9 @@ bool
 Geofence::valid()
 {
 	// NULL fence is valid
-	if (isEmpty())
+	if (isEmpty()) {
 		return true;
+	}
 
 	// Otherwise
 	if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) {
@@ -214,26 +225,33 @@ Geofence::addPoint(int argc, char *argv[])
 		return;
 	}
 
-	if (argc < 3)
+	if (argc < 3) {
 		errx(1, "Specify: -clear | sequence latitude longitude [-publish]");
+	}
 
 	ix = atoi(argv[0]);
-	if (ix >= DM_KEY_FENCE_POINTS_MAX)
+
+	if (ix >= DM_KEY_FENCE_POINTS_MAX) {
 		errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX);
+	}
 
 	lat = strtod(argv[1], &end);
 	lon = strtod(argv[2], &end);
 
 	last = 0;
-	if ((argc > 3) && (strcmp(argv[3], "-publish") == 0))
+
+	if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) {
 		last = 1;
+	}
 
 	vertex.lat = (float)lat;
 	vertex.lon = (float)lon;
 
 	if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) {
-		if (last)
+		if (last) {
 			publishFence((unsigned)ix + 1);
+		}
+
 		return;
 	}
 
@@ -243,10 +261,12 @@ Geofence::addPoint(int argc, char *argv[])
 void
 Geofence::publishFence(unsigned vertices)
 {
-	if (_fence_pub == -1)
+	if (_fence_pub == -1) {
 		_fence_pub = orb_advertise(ORB_ID(fence), &vertices);
-	else
+
+	} else {
 		orb_publish(ORB_ID(fence), _fence_pub, &vertices);
+	}
 }
 
 int
@@ -257,26 +277,29 @@ Geofence::loadFromFile(const char *filename)
 	int			pointCounter = 0;
 	bool		gotVertical = false;
 	const char commentChar = '#';
+	int rc = ERROR;
 
 	/* Make sure no data is left in the datamanager */
 	clearDm();
 
 	/* open the mixer definition file */
 	fp = fopen(GEOFENCE_FILENAME, "r");
+
 	if (fp == NULL) {
 		return ERROR;
 	}
 
 	/* create geofence points from valid lines and store in DM */
 	for (;;) {
-
 		/* get a line, bail on error/EOF */
-		if (fgets(line, sizeof(line), fp) == NULL)
+		if (fgets(line, sizeof(line), fp) == NULL) {
 			break;
+		}
 
 		/* Trim leading whitespace */
 		size_t textStart = 0;
-		while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++;
+
+		while ((textStart < sizeof(line) / sizeof(char)) && isspace(line[textStart])) { textStart++; }
 
 		/* if the line starts with #, skip */
 		if (line[textStart] == commentChar) {
@@ -299,55 +322,56 @@ Geofence::loadFromFile(const char *filename)
 
 				if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) {
 					warnx("Scanf to parse DMS geofence vertex failed.");
-					return ERROR;
+					goto error;
 				}
 
 //				warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s);
 
-				vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f;
-				vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f;
+				vertex.lat = lat_d + lat_m / 60.0f + lat_s / 3600.0f;
+				vertex.lon = lon_d + lon_m / 60.0f + lon_s / 3600.0f;
 
 			} else {
 				/* Handle decimal degree format */
 				if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) {
 					warnx("Scanf to parse geofence vertex failed.");
-					return ERROR;
+					goto error;
 				}
 			}
 
-			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))
-								return ERROR;
+			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) {
+				goto error;
+			}
 
-			warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter,  (double)vertex.lat, (double)vertex.lon);
+			warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon);
 
 			pointCounter++;
+
 		} else {
 			/* Parse the line as the vertical limits */
-			if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2)
-				return ERROR;
-
+			if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) {
+				goto error;
+			}
 
 			warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max);
 			gotVertical = true;
 		}
-
-
 	}
 
-	fclose(fp);
-
 	/* Check if import was successful */
-	if(gotVertical && pointCounter > 0)
-	{
+	if (gotVertical && pointCounter > 0) {
 		_verticesCount = pointCounter;
 		warnx("Geofence: imported successfully");
 		mavlink_log_info(_mavlinkFd, "Geofence imported");
+		rc = OK;
+
 	} else {
 		warnx("Geofence: import error");
 		mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error");
 	}
 
-	return ERROR;
+error:
+	fclose(fp);
+	return rc;
 }
 
 int Geofence::clearDm()
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 6de283396..ec297e7f0 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -877,6 +877,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
 		float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
 		float w_z_gps_p = params.w_z_gps_p * w_gps_z;
+		float w_z_gps_v = params.w_z_gps_v * w_gps_z;
 
 		float w_xy_vision_p = params.w_xy_vision_p;
 		float w_xy_vision_v = params.w_xy_vision_v;
@@ -907,6 +908,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 		if (use_gps_z) {
 			accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
+			accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;
 		}
 
 		/* transform error vector from NED frame to body frame */
@@ -991,6 +993,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			epv = fminf(epv, gps.epv);
 
 			inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
+			inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);
 		}
 
 		if (use_vision_z) {
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index b7f6509d7..5387b7e87 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -64,6 +64,17 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
 
 /**
+ * Z velocity weight for GPS
+ *
+ * Weight (cutoff frequency) for GPS altitude velocity measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
+PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f);
+
+/**
  * Z axis weight for vision
  *
  * Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset.
@@ -281,6 +292,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
 {
 	h->w_z_baro = param_find("INAV_W_Z_BARO");
 	h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
+	h->w_z_gps_v = param_find("INAV_W_Z_GPS_V");
 	h->w_z_vision_p = param_find("INAV_W_Z_VIS_P");
 	h->w_z_sonar = param_find("INAV_W_Z_SONAR");
 	h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index d0a65e42e..51bbda412 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -44,6 +44,7 @@
 struct position_estimator_inav_params {
 	float w_z_baro;
 	float w_z_gps_p;
+	float w_z_gps_v;
 	float w_z_vision_p;
 	float w_z_sonar;
 	float w_xy_gps_p;
@@ -68,6 +69,7 @@ struct position_estimator_inav_params {
 struct position_estimator_inav_param_handles {
 	param_t w_z_baro;
 	param_t w_z_gps_p;
+	param_t w_z_gps_v;
 	param_t w_z_vision_p;
 	param_t w_z_sonar;
 	param_t w_xy_gps_p;
diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c
index 4bcf95784..24f4e4207 100644
--- a/src/modules/systemlib/mcu_version.c
+++ b/src/modules/systemlib/mcu_version.c
@@ -47,7 +47,8 @@
 #ifdef CONFIG_ARCH_CHIP_STM32
 #include <up_arch.h>
 
-#define DBGMCU_IDCODE	0xE0042000
+#define DBGMCU_IDCODE	0xE0042000  //STM DocID018909 Rev 8 Sect 38.18 (MCU device ID code)
+#define UNIQUE_ID    	0x1FFF7A10  //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register)
 
 #define STM32F40x_41x	0x413
 #define STM32F42x_43x	0x419
@@ -57,7 +58,13 @@
 
 #endif
 
-
+/** Copy the 96bit MCU Unique ID into the provided pointer */
+void mcu_unique_id(uint32_t *uid_96_bit)
+{
+	uid_96_bit[0] = getreg32(UNIQUE_ID);
+	uid_96_bit[1] = getreg32(UNIQUE_ID+4);
+	uid_96_bit[2] = getreg32(UNIQUE_ID+8);
+}
 
 int mcu_version(char* rev, char** revstr)
 {
diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h
index 1b3d0aba9..c8a0bf5cd 100644
--- a/src/modules/systemlib/mcu_version.h
+++ b/src/modules/systemlib/mcu_version.h
@@ -33,6 +33,8 @@
 
 #pragma once
 
+#include <stdint.h>
+
 /* magic numbers from reference manual */
 enum MCU_REV {
 	MCU_REV_STM32F4_REV_A = 0x1000,
@@ -42,6 +44,15 @@ enum MCU_REV {
 	MCU_REV_STM32F4_REV_3 = 0x2001
 };
 
+
+/**
+ * Reports the microcontroller unique id.
+ *
+ * This ID is guaranteed to be unique for every mcu. 
+ * @param uid_96_bit A uint32_t[3] array to copy the data to.
+ */
+__EXPORT void mcu_unique_id(uint32_t *uid_96_bit);
+
 /**
  * Reports the microcontroller version of the main CPU.
  *
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 9f682c7e1..51589e43e 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -49,6 +49,13 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_uavcan_sub_status(node),
 	_orb_timer(node)
 {
+	if (_perfcnt_invalid_input == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input");
+	}
+
+	if (_perfcnt_scaling_error == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error");
+	}
 }
 
 UavcanEscController::~UavcanEscController()
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index e5d30f6c4..600cb47f3 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -38,7 +38,7 @@
 
 MODULE_COMMAND = uavcan
 
-MAXOPTIMIZATION = -Os
+MAXOPTIMIZATION = -O3
 
 # Main
 SRCS += uavcan_main.cpp              \
@@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp   \
 #
 # libuavcan
 #
-include $(PX4_LIB_DIR)/uavcan/libuavcan/include.mk
+include $(PX4_LIB_DIR)uavcan/libuavcan/include.mk
 SRCS += $(LIBUAVCAN_SRC)
 INCLUDE_DIRS += $(LIBUAVCAN_INC)
 # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
@@ -67,7 +67,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV
 #
 # libuavcan drivers for STM32
 #
-include $(PX4_LIB_DIR)/uavcan/libuavcan_drivers/stm32/driver/include.mk
+include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/stm32/driver/include.mk
 SRCS += $(LIBUAVCAN_STM32_SRC)
 INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
 override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 60901e0c7..4dc03b61b 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -81,6 +81,18 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	if (res < 0) {
 		std::abort();
 	}
+
+	if (_perfcnt_node_spin_elapsed == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_node_spin_elapsed");
+	}
+
+	if (_perfcnt_esc_mixer_output_elapsed == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_output_elapsed");
+	}
+
+	if (_perfcnt_esc_mixer_total_elapsed == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_total_elapsed");
+	}
 }
 
 UavcanNode::~UavcanNode()
@@ -118,6 +130,10 @@ UavcanNode::~UavcanNode()
 	}
 
 	_instance = nullptr;
+
+	perf_free(_perfcnt_node_spin_elapsed);
+	perf_free(_perfcnt_esc_mixer_output_elapsed);
+	perf_free(_perfcnt_esc_mixer_total_elapsed);
 }
 
 int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
@@ -265,10 +281,12 @@ int UavcanNode::init(uavcan::NodeID node_id)
 
 void UavcanNode::node_spin_once()
 {
+	perf_begin(_perfcnt_node_spin_elapsed);
 	const int spin_res = _node.spin(uavcan::MonotonicTime());
 	if (spin_res < 0) {
 		warnx("node spin error %i", spin_res);
 	}
+	perf_end(_perfcnt_node_spin_elapsed);
 }
 
 /*
@@ -344,8 +362,12 @@ int UavcanNode::run()
 		// Mutex is unlocked while the thread is blocked on IO multiplexing
 		(void)pthread_mutex_unlock(&_node_mutex);
 
+		perf_end(_perfcnt_esc_mixer_total_elapsed); // end goes first, it's not a mistake
+
 		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
 
+		perf_begin(_perfcnt_esc_mixer_total_elapsed);
+
 		(void)pthread_mutex_lock(&_node_mutex);
 
 		node_spin_once();  // Non-blocking
@@ -430,7 +452,9 @@ int UavcanNode::run()
 			}
 			// Output to the bus
 			_outputs.timestamp = hrt_absolute_time();
+			perf_begin(_perfcnt_esc_mixer_output_elapsed);
 			_esc_controller.update_outputs(_outputs.output, _outputs.noutputs);
+			perf_end(_perfcnt_esc_mixer_output_elapsed);
 		}
 
 
@@ -484,7 +508,7 @@ UavcanNode::teardown()
 			_control_subs[i] = -1;
 		}
 	}
-	return ::close(_armed_sub);
+	return (_armed_sub >= 0) ? ::close(_armed_sub) : 0;
 }
 
 int
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 98f2e5ad4..19b9b4b48 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -34,9 +34,9 @@
 #pragma once
 
 #include <nuttx/config.h>
-
 #include <uavcan_stm32/uavcan_stm32.hpp>
 #include <drivers/device/device.h>
+#include <systemlib/perf_counter.h>
 
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
@@ -66,7 +66,7 @@
  */
 class UavcanNode : public device::CDev
 {
-	static constexpr unsigned MemPoolSize        = 10752;
+	static constexpr unsigned MemPoolSize        = 10752; ///< Refer to the libuavcan manual to learn why
 	static constexpr unsigned RxQueueLenPerIface = 64;
 	static constexpr unsigned StackSize          = 3000;
 
@@ -107,11 +107,11 @@ private:
 	int			_task = -1;			///< handle to the OS task
 	bool			_task_should_exit = false;	///< flag to indicate to tear down the CAN driver
 	int			_armed_sub = -1;		///< uORB subscription of the arming status
-	actuator_armed_s	_armed;				///< the arming request of the system
+	actuator_armed_s	_armed = {};			///< the arming request of the system
 	bool			_is_armed = false;		///< the arming status of the actuators on the bus
 
 	int			_test_motor_sub = -1;   ///< uORB subscription of the test_motor status
-	test_motor_s		_test_motor;
+	test_motor_s		_test_motor = {};
 	bool			_test_in_progress = false;
 
 	unsigned		_output_count = 0;		///< number of actuators currently available
@@ -135,11 +135,15 @@ private:
 	unsigned		_poll_fds_num = 0;
 
 	int			_actuator_direct_sub = -1;   ///< uORB subscription of the actuator_direct topic
-	uint8_t			_actuator_direct_poll_fd_num;
-	actuator_direct_s	_actuator_direct;
+	uint8_t			_actuator_direct_poll_fd_num = 0;
+	actuator_direct_s	_actuator_direct = {};
 
-	actuator_outputs_s	_outputs;
+	actuator_outputs_s	_outputs = {};
 
 	// index into _poll_fds for each _control_subs handle
 	uint8_t			_poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
+
+	perf_counter_t _perfcnt_node_spin_elapsed        = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed");
+	perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed");
+	perf_counter_t _perfcnt_esc_mixer_total_elapsed  = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed");
 };
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 077bc47c9..9f13edb18 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -36,7 +36,7 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  *
- * config tool.
+ * config tool. Takes the device name as the first parameter.
  */
 
 #include <nuttx/config.h>
@@ -71,18 +71,18 @@ int
 config_main(int argc, char *argv[])
 {
 	if (argc >= 2) {
-		if (!strcmp(argv[1], "gyro")) {
-			do_gyro(argc - 2, argv + 2);
-		} else if (!strcmp(argv[1], "accel")) {
-			do_accel(argc - 2, argv + 2);
-		} else if (!strcmp(argv[1], "mag")) {
-			do_mag(argc - 2, argv + 2);
+		if (!strncmp(argv[1], "/dev/gyro",9)) {
+			do_gyro(argc - 1, argv + 1);
+		} else if (!strncmp(argv[1], "/dev/accel",10)) {
+			do_accel(argc - 1, argv + 1);
+		} else if (!strncmp(argv[1], "/dev/mag",8)) {
+			do_mag(argc - 1, argv + 1);
 		} else {
 			do_device(argc - 1, argv + 1);
 		}
 	}
 	
-	errx(1, "expected a command, try 'gyro', 'accel', 'mag'");
+	errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'");
 }
 
 static void
@@ -133,41 +133,41 @@ do_gyro(int argc, char *argv[])
 {
 	int	fd;
 
-	fd = open(GYRO_DEVICE_PATH, 0);
+	fd = open(argv[0], 0);
 
 	if (fd < 0) {
-		warn("%s", GYRO_DEVICE_PATH);
+		warn("%s", argv[0]);
 		errx(1, "FATAL: no gyro found");
 
 	} else {
 
 		int ret;
 
-		if (argc == 2 && !strcmp(argv[0], "sampling")) {
+		if (argc == 3 && !strcmp(argv[1], "sampling")) {
 
 			/* set the gyro internal sampling rate up to at least i Hz */
-			ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"sampling rate could not be set");
 
-		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
+		} else if (argc == 3 && !strcmp(argv[1], "rate")) {
 
 			/* set the driver to poll at i Hz */
-			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"pollrate could not be set");
 
-		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+		} else if (argc == 3 && !strcmp(argv[1], "range")) {
 
 			/* set the range to i dps */
-			ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"range could not be set");
 
-		} else if (argc == 1 && !strcmp(argv[0], "check")) {
+		} else if (argc == 2 && !strcmp(argv[1], "check")) {
 			ret = ioctl(fd, GYROIOCSELFTEST, 0);
 
 			if (ret) {
@@ -206,41 +206,41 @@ do_mag(int argc, char *argv[])
 {
 	int fd;
 
-	fd = open(MAG_DEVICE_PATH, 0);
+	fd = open(argv[0], 0);
 
 	if (fd < 0) {
-		warn("%s", MAG_DEVICE_PATH);
+		warn("%s", argv[0]);
 		errx(1, "FATAL: no magnetometer found");
 
 	} else {
 
 		int ret;
 
-		if (argc == 2 && !strcmp(argv[0], "sampling")) {
+		if (argc == 3 && !strcmp(argv[1], "sampling")) {
 
 			/* set the mag internal sampling rate up to at least i Hz */
-			ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"sampling rate could not be set");
 
-		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
+		} else if (argc == 3 && !strcmp(argv[1], "rate")) {
 
 			/* set the driver to poll at i Hz */
-			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"pollrate could not be set");
 
-		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+		} else if (argc == 3 && !strcmp(argv[1], "range")) {
 
 			/* set the range to i G */
-			ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"range could not be set");
 
-		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+		} else if(argc == 2 && !strcmp(argv[1], "check")) {
 			ret = ioctl(fd, MAGIOCSELFTEST, 0);
 
 			if (ret) {
@@ -282,41 +282,41 @@ do_accel(int argc, char *argv[])
 {
 	int	fd;
 
-	fd = open(ACCEL_DEVICE_PATH, 0);
+	fd = open(argv[0], 0);
 
 	if (fd < 0) {
-		warn("%s", ACCEL_DEVICE_PATH);
+		warn("%s", argv[0]);
 		errx(1, "FATAL: no accelerometer found");
 
 	} else {
 
 		int ret;
 
-		if (argc == 2 && !strcmp(argv[0], "sampling")) {
+		if (argc == 3 && !strcmp(argv[1], "sampling")) {
 
 			/* set the accel internal sampling rate up to at least i Hz */
-			ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"sampling rate could not be set");
 
-		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
+		} else if (argc == 3 && !strcmp(argv[1], "rate")) {
 
 			/* set the driver to poll at i Hz */
-			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"pollrate could not be set");
 
-		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+		} else if (argc == 3 && !strcmp(argv[1], "range")) {
 
 			/* set the range to i G */
-			ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"range could not be set");
 
-		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+		} else if(argc == 2 && !strcmp(argv[1], "check")) {
 			ret = ioctl(fd, ACCELIOCSELFTEST, 0);
 
 			if (ret) {
diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk
index a12bc369e..4e2710572 100644
--- a/src/systemcmds/nshterm/module.mk
+++ b/src/systemcmds/nshterm/module.mk
@@ -38,7 +38,7 @@
 MODULE_COMMAND	 = nshterm
 SRCS		 = nshterm.c
 
-MODULE_STACKSIZE = 1600
+MODULE_STACKSIZE = 1500
 
 MAXOPTIMIZATION	 = -Os
 
diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c
index ceaea35b6..50547a562 100644
--- a/src/systemcmds/nshterm/nshterm.c
+++ b/src/systemcmds/nshterm/nshterm.c
@@ -50,6 +50,7 @@
 #include <apps/nsh.h>
 #include <fcntl.h>
 #include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
 
 #include <uORB/topics/actuator_armed.h>
 
@@ -67,6 +68,11 @@ nshterm_main(int argc, char *argv[])
     int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
     struct actuator_armed_s armed;
 
+    /* back off 800 ms to avoid running into the USB setup timing */
+    while (hrt_absolute_time() < 800U * 1000U) {
+        usleep(50000);
+    }
+
     /* try to bring up the console - stop doing so if the system gets armed */
     while (true) {
 
diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 2ead3e632..087eb52e3 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -54,6 +54,7 @@ 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 const char mcu_ver_str[]		= "mcu";
+static const char mcu_uid_str[]		= "uid";
 
 static void usage(const char *reason)
 {
@@ -61,7 +62,7 @@ static void usage(const char *reason)
 		printf("%s\n", reason);
 	}
 
-	printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n");
+	printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu|uid}\n\n");
 }
 
 __EXPORT int ver_main(int argc, char *argv[]);
@@ -141,6 +142,17 @@ int ver_main(int argc, char *argv[])
 				ret = 0;
 			}
 
+			if (show_all || !strncmp(argv[1], mcu_uid_str, sizeof(mcu_uid_str))) {
+				uint32_t uid[3];
+
+				mcu_unique_id(uid);
+
+				printf("UID: %X:%X:%X \n",uid[0],uid[1],uid[2]);
+
+				ret = 0;
+			}
+
+
 			if (ret == 1) {
 				errx(1, "unknown command.\n");
 			}