Merge pull request #1734 from PX4/ros_messagelayer_merge_attctrl_posctrl

Ros messagelayer merge attctrl posctrl

.gitignore

@@ -40,6 +40,8 @@ tags
 .ropeproject
 *.orig
 src/modules/uORB/topics/*
+src/platforms/nuttx/px4_messages/*
+src/platforms/ros/px4_messages/*
 Firmware.zip
 unittests/build
 *.generated.h

.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:

CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(px4)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 add_definitions(-D__PX4_ROS)
+add_definitions(-D__EXPORT=)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -68,6 +69,11 @@ add_message_files(
   actuator_armed.msg
   parameter_update.msg
   vehicle_status.msg
+  vehicle_local_position.msg
+  position_setpoint.msg
+  position_setpoint_triplet.msg
+  vehicle_local_position_setpoint.msg
+  vehicle_global_velocity_setpoint.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -116,6 +122,7 @@ catkin_package(
 include_directories(
   ${catkin_INCLUDE_DIRS}
   src/platforms
+  src/platforms/ros/px4_messages
   src/include
   src/modules
   src/
@@ -123,6 +130,16 @@ include_directories(
   ${EIGEN_INCLUDE_DIRS}
 )
 
+## generate multiplatform wrapper headers
+## note that the message header files are generated as in any ROS project with generate_messages()
+set(MULTIPLATFORM_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/platforms/ros/px4_messages)
+set(MULTIPLATFORM_TEMPLATE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg/templates/px4/ros)
+set(TOPICHEADER_TEMP_DIR ${CMAKE_BINARY_DIR}/topics_temporary)
+set(MULTIPLATFORM_PREFIX px4_)
+add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_headers.py
+	-d ${CMAKE_CURRENT_SOURCE_DIR}/msg -o ${MULTIPLATFORM_HEADER_DIR} -e ${MULTIPLATFORM_TEMPLATE_DIR}
+	-t ${TOPICHEADER_TEMP_DIR} -p ${MULTIPLATFORM_PREFIX})
+
 ## Declare a cpp library
 add_library(px4
   src/platforms/ros/px4_ros_impl.cpp
@@ -131,7 +148,7 @@ add_library(px4
   src/lib/mathlib/math/Limits.cpp
   src/modules/systemlib/circuit_breaker.cpp
 )
-add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp)
+add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
 
 target_link_libraries(px4
   ${catkin_LIBRARIES}
@@ -141,7 +158,7 @@ target_link_libraries(px4
 add_executable(publisher
 	src/examples/publisher/publisher_main.cpp
 	src/examples/publisher/publisher_example.cpp)
-add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp)
+add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
 target_link_libraries(publisher
   ${catkin_LIBRARIES}
   px4
@@ -151,7 +168,7 @@ target_link_libraries(publisher
 add_executable(subscriber
 	src/examples/subscriber/subscriber_main.cpp
 	src/examples/subscriber/subscriber_example.cpp)
-add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp)
+add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
 target_link_libraries(subscriber
   ${catkin_LIBRARIES}
   px4
@@ -168,6 +185,16 @@ target_link_libraries(mc_att_control
   px4
 )
 
+## MC Position Control
+add_executable(mc_pos_control
+	src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
+	src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp)
+add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mc_pos_control
+  ${catkin_LIBRARIES}
+  px4
+)
+
 ## Attitude Estimator dummy
 add_executable(attitude_estimator
 	src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
@@ -177,6 +204,15 @@ target_link_libraries(attitude_estimator
   px4
 )
 
+## Position Estimator dummy
+add_executable(position_estimator
+	src/platforms/ros/nodes/position_estimator/position_estimator.cpp)
+add_dependencies(position_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(position_estimator
+  ${catkin_LIBRARIES}
+  px4
+)
+
 ## Manual input
 add_executable(manual_input
 	src/platforms/ros/nodes/manual_input/manual_input.cpp)

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

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

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
@@ -225,20 +227,27 @@ updatesubmodules:
 	$(Q) (git submodule update)
 
 MSG_DIR = $(PX4_BASE)msg
-MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates
+UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb
+MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4/uorb
 TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics
-TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary
-GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src
-GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src
+MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages
+MULTIPLATFORM_PREFIX = px4_
+TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary
+GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src
+GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src
 
 .PHONY: generateuorbtopicheaders
 generateuorbtopicheaders:
 	@$(ECHO) "Generating uORB topic headers"
 	$(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \
 		$(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \
-		-d $(MSG_DIR) -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR))
+		-d $(MSG_DIR) -o $(TOPICS_DIR) -e $(UORB_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR))
+	@$(ECHO) "Generating multiplatform uORB topic wrapper headers"
+	$(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \
+		$(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \
+		-d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX))
 # clean up temporary files
-	$(Q) (rm -r $(TOPICS_TEMPORARY_DIR))
+	$(Q) (rm -r $(TOPICHEADER_TEMP_DIR))
 
 #
 # Testing targets

NuttX

@@ -1 +1 @@
-Subproject commit 3d8171f6ea88297d8595525c8222d61e9cf20fd0
+Subproject commit 37cbc3e8ae6b75e9b7e79996d30fe8ed0beb9704

README.md

@@ -1,6 +1,6 @@
 ## PX4 Flight Control Stack and Middleware ##
 
-[![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware)
+[![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) [![Coverity Scan](https://scan.coverity.com/projects/3966/badge.svg?flat=1)](https://scan.coverity.com/projects/3966?tab=overview)
 
 [![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge)
 
@@ -21,7 +21,8 @@ Please refer to the [user documentation](https://pixhawk.org/users/start) for fl
 ### Developers ###
 
 Contributing guide:
-http://px4.io/dev/contributing
+  * [CONTRIBUTING.md](https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md)
+  * [PX4 Contribution Guide](http://px4.io/dev/contributing)
 
 Developer guide:
 http://px4.io/dev/

ROMFS/px4fmu_common/init.d/rc.fw_apps

@@ -13,3 +13,12 @@ ekf_att_pos_estimator start
 #
 fw_att_control start
 fw_pos_control_l1 start
+
+#
+# Start Land Detector
+#
+land_detector start fixedwing
+
+#
+# Misc apps
+#

ROMFS/px4fmu_common/init.d/rc.logging

@@ -8,7 +8,7 @@ then
 	if ver hwcmp PX4FMU_V1
 	then
 		echo "Start sdlog2 at 50Hz"
-		sdlog2 start -r 50 -a -b 4 -t
+		sdlog2 start -r 40 -a -b 3 -t
 	else
 		echo "Start sdlog2 at 200Hz"
 		sdlog2 start -r 200 -a -b 22 -t

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -14,4 +14,15 @@ else
 	# try the multiplatform version
 	mc_att_control_m start
 fi
-mc_pos_control start
+
+if mc_pos_control start
+then
+else
+	# try the multiplatform version
+	mc_pos_control_m start
+fi
+
+#
+# Start Land Detector
+#
+land_detector start multicopter

ROMFS/px4fmu_common/init.d/rcS

@@ -5,6 +5,11 @@
 #  NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
 #
 
+#
+# Start CDC/ACM serial driver
+#
+sercon
+
 #
 # Default to auto-start mode.
 #
@@ -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 &
 

ROMFS/px4fmu_test/init.d/rcS

@@ -97,6 +97,13 @@ else
 	set unit_test_failure_list "${unit_test_failure_list} commander_tests"
 fi
 
+if uorb test
+then
+else
+	set unit_test_failure 1
+	set unit_test_failure_list "${unit_test_failure_list} uorb_tests"
+fi
+
 if hmc5883 -I start
 then
 	# This is an FMUv3

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

Tools/px_generate_uorb_topic_headers.py

@@ -60,7 +60,7 @@ def convert_file(filename, outputdir, templatedir, includepath):
         """
         Converts a single .msg file to a uorb header
         """
-        print("Generating uORB headers from {0}".format(filename))
+        print("Generating headers from {0}".format(filename))
         genmsg.template_tools.generate_from_file(filename,
                                                  package,
                                                  outputdir,
@@ -85,16 +85,21 @@ def convert_dir(inputdir, outputdir, templatedir):
                             includepath)
 
 
-def copy_changed(inputdir, outputdir):
+def copy_changed(inputdir, outputdir, prefix=''):
         """
         Copies files from inputdir to outputdir if they don't exist in
         ouputdir or if their content changed
         """
+
+        # Make sure output directory exists:
+        if not os.path.isdir(outputdir):
+                os.makedirs(outputdir)
+
         for f in os.listdir(inputdir):
                 fni = os.path.join(inputdir, f)
                 if os.path.isfile(fni):
                         # Check if f exists in outpoutdir, copy the file if not
-                        fno = os.path.join(outputdir, f)
+                        fno = os.path.join(outputdir, prefix + f)
                         if not os.path.isfile(fno):
                                 shutil.copy(fni, fno)
                                 print("{0}: new header file".format(f))
@@ -108,7 +113,8 @@ def copy_changed(inputdir, outputdir):
 
                         print("{0}: unchanged".format(f))
 
-def convert_dir_save(inputdir, outputdir, templatedir, temporarydir):
+
+def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix):
         """
         Converts all .msg files in inputdir to uORB header files
         Unchanged existing files are not overwritten.
@@ -117,7 +123,7 @@ def convert_dir_save(inputdir, outputdir, templatedir, temporarydir):
         convert_dir(inputdir, temporarydir, templatedir)
 
         # Copy changed headers from temporary dir to output dir
-        copy_changed(temporarydir, outputdir)
+        copy_changed(temporarydir, outputdir, prefix)
 
 if __name__ == "__main__":
         parser = argparse.ArgumentParser(
@@ -132,6 +138,9 @@ if __name__ == "__main__":
                             help='output directory for header files')
         parser.add_argument('-t', dest='temporarydir',
                             help='temporary directory')
+        parser.add_argument('-p', dest='prefix', default='',
+                            help='string added as prefix to the output file '
+                            ' name when converting directories')
         args = parser.parse_args()
 
         if args.file is not None:
@@ -146,4 +155,5 @@ if __name__ == "__main__":
                     args.dir,
                     args.outputdir,
                     args.templatedir,
-                    args.temporarydir)
+                    args.temporarydir,
+                    args.prefix)

Tools/px_uploader.py

@@ -266,18 +266,19 @@ class uploader(object):
                 self.__getSync()
                 return value
 
-        def __drawProgressBar(self, progress, maxVal):
+        def __drawProgressBar(self, label, progress, maxVal):
                 if maxVal < progress:
                     progress = maxVal
 
                 percent = (float(progress) / float(maxVal)) * 100.0
 
-                sys.stdout.write("\rprogress:[%-20s] %.2f%%" % ('='*int(percent/5.0), percent))
+                sys.stdout.write("\r%s: [%-20s] %.1f%%" % (label, '='*int(percent/5.0), percent))
                 sys.stdout.flush()
 
 
         # send the CHIP_ERASE command and wait for the bootloader to become ready
-        def __erase(self):
+        def __erase(self, label):
+                print("\n", end='')
                 self.__send(uploader.CHIP_ERASE
                             + uploader.EOC)
 
@@ -288,15 +289,14 @@ class uploader(object):
                         #Draw progress bar (erase usually takes about 9 seconds to complete)
                         estimatedTimeRemaining = deadline-time.time()
                         if estimatedTimeRemaining >= 9.0:
-                            self.__drawProgressBar(20.0-estimatedTimeRemaining, 9.0)
+                            self.__drawProgressBar(label, 20.0-estimatedTimeRemaining, 9.0)
                         else:
-                            self.__drawProgressBar(10.0, 10.0)
+                            self.__drawProgressBar(label, 10.0, 10.0)
                             sys.stdout.write(" (timeout: %d seconds) " % int(deadline-time.time()) )
                             sys.stdout.flush()
 
                         if self.__trySync():
-                            self.__drawProgressBar(10.0, 10.0)
-                            sys.stdout.write("\nerase complete!\n")
+                            self.__drawProgressBar(label, 10.0, 10.0)
                             return;
 
                 raise RuntimeError("timed out waiting for erase")
@@ -350,7 +350,8 @@ class uploader(object):
                 return [seq[i:i+length] for i in range(0, len(seq), length)]
 
         # upload code
-        def __program(self, fw):
+        def __program(self, label, fw):
+                print("\n", end='')
                 code = fw.image
                 groups = self.__split_len(code, uploader.PROG_MULTI_MAX)
 
@@ -361,31 +362,40 @@ class uploader(object):
                         #Print upload progress (throttled, so it does not delay upload progress)
                         uploadProgress += 1
                         if uploadProgress % 256 == 0:
-                            self.__drawProgressBar(uploadProgress, len(groups))
-                self.__drawProgressBar(100, 100)
-                print("\nprogram complete!")
+                            self.__drawProgressBar(label, uploadProgress, len(groups))
+                self.__drawProgressBar(label, 100, 100)
 
         # verify code
-        def __verify_v2(self, fw):
+        def __verify_v2(self, label, fw):
+                print("\n", end='')
                 self.__send(uploader.CHIP_VERIFY
                             + uploader.EOC)
                 self.__getSync()
                 code = fw.image
                 groups = self.__split_len(code, uploader.READ_MULTI_MAX)
+                verifyProgress = 0
                 for bytes in groups:
+                        verifyProgress += 1
+                        if verifyProgress % 256 == 0:
+                            self.__drawProgressBar(label, verifyProgress, len(groups))
                         if (not self.__verify_multi(bytes)):
                                 raise RuntimeError("Verification failed")
+                self.__drawProgressBar(label, 100, 100)
 
-        def __verify_v3(self, fw):
-                expect_crc = fw.crc(self.fw_maxsize)
+        def __verify_v3(self, label, fw):
+                print("\n", end='')
+                self.__drawProgressBar(label, 1, 100)
+                expect_crc = fw.crc(self.fw_maxsize)                
                 self.__send(uploader.GET_CRC
                             + uploader.EOC)
                 report_crc = self.__recv_int()
                 self.__getSync()
+                verifyProgress = 0
                 if report_crc != expect_crc:
                         print("Expected 0x%x" % expect_crc)
                         print("Got      0x%x" % report_crc)
                         raise RuntimeError("Program CRC failed")
+                self.__drawProgressBar(label, 100, 100)
 
         # get basic data about the board
         def identify(self):
@@ -439,19 +449,16 @@ class uploader(object):
                     except Exception:
                             # ignore bad character encodings
                             pass
-                print("erase...")
-                self.__erase()
+                
+                self.__erase("Erase  ")
+                self.__program("Program", fw)
 
-                print("program...")
-                self.__program(fw)
-
-                print("verify...")
                 if self.bl_rev == 2:
-                        self.__verify_v2(fw)
+                        self.__verify_v2("Verify ", fw)
                 else:
-                        self.__verify_v3(fw)
+                        self.__verify_v3("Verify ", fw)
 
-                print("done, rebooting.")
+                print("\nRebooting.\n")
                 self.__reboot()
                 self.port.close()
                 

launch/multicopter.launch

@@ -6,7 +6,9 @@
 	<node pkg="px4" name="commander" type="commander"/>
 	<node pkg="px4" name="mc_mixer" type="mc_mixer"/>
 	<node pkg="px4" name="attitude_estimator" type="attitude_estimator"/>
+	<node pkg="px4" name="position_estimator" type="position_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
+	<node pkg="px4" name="mc_pos_control" type="mc_pos_control"/>
 </group>
 
 </launch>

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)

makefiles/config_px4fmu-v2_default.mk

@@ -71,6 +71,7 @@ MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
 MODULES		+= modules/uavcan
+MODULES 	+= modules/land_detector
 
 #
 # Estimation modules (EKF/ SO3 / other filters)

makefiles/config_px4fmu-v2_multiplatform.mk

@@ -87,7 +87,10 @@ MODULES		+= modules/position_estimator_inav
 #MODULES		+= modules/fw_att_control
 # MODULES		+= modules/mc_att_control
 MODULES		+= modules/mc_att_control_multiplatform
-MODULES		+= modules/mc_pos_control
+MODULES		+= examples/subscriber
+MODULES		+= examples/publisher
+# MODULES		+= modules/mc_pos_control
+MODULES		+= modules/mc_pos_control_multiplatform
 MODULES 	+= modules/vtol_att_control
 
 #

makefiles/config_px4fmu-v2_test.mk

@@ -32,6 +32,7 @@ MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/ver
+MODULES		+= systemcmds/top
 
 #
 # System commands

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

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 \

msg/actuator_controls_1.msg

@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control

msg/actuator_controls_2.msg

@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control

msg/actuator_controls_3.msg

@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control

msg/actuator_controls_virtual_fw.msg

@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control

msg/fw_virtual_rates_setpoint.msg

@@ -0,0 +1,6 @@
+uint64 timestamp    # in microseconds since system start
+
+float32 roll	    # body angular rates in NED frame
+float32 pitch	    # body angular rates in NED frame
+float32 yaw	    # body angular rates in NED frame
+float32 thrust	    # thrust normalized to 0..1

msg/position_setpoint.msg

@@ -0,0 +1,35 @@
+# this file is only used in the position_setpoint triple as a dependency
+
+uint8 SETPOINT_TYPE_POSITION=0	# position setpoint
+uint8 SETPOINT_TYPE_VELOCITY=1	# velocity setpoint
+uint8 SETPOINT_TYPE_LOITER=2	# loiter setpoint
+uint8 SETPOINT_TYPE_TAKEOFF=3	# takeoff setpoint
+uint8 SETPOINT_TYPE_LAND=4	# land setpoint, altitude must be ignored, descend until landing
+uint8 SETPOINT_TYPE_IDLE=5	# do nothing, switch off motors or keep at idle speed (MC)
+uint8 SETPOINT_TYPE_OFFBOARD=6 	# setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard
+
+bool valid			# true if setpoint is valid
+uint8 type			# setpoint type to adjust behavior of position controller
+float32 x			# local position setpoint in m in NED
+float32 y			# local position setpoint in m in NED
+float32 z			# local position setpoint in m in NED
+bool position_valid		# true if local position setpoint valid
+float32 vx			# local velocity setpoint in m/s in NED
+float32 vy			# local velocity setpoint in m/s in NED
+float32 vz			# local velocity setpoint in m/s in NED
+bool velocity_valid		# true if local velocity setpoint valid
+float64 lat			# latitude, in deg
+float64 lon			# longitude, in deg
+float32 alt			# altitude AMSL, in m
+float32 yaw			# yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw
+bool yaw_valid			# true if yaw setpoint valid
+float32 yawspeed		# yawspeed (only for multirotors, in rad/s)
+bool yawspeed_valid		# true if yawspeed setpoint valid
+float32 loiter_radius		# loiter radius (only for fixed wing), in m
+int8 loiter_direction		# loiter direction: 1 = CW, -1 = CCW
+float32 pitch_min		# minimal pitch angle for fixed wing takeoff waypoints
+float32 a_x			# acceleration x setpoint
+float32 a_y			# acceleration y setpoint
+float32 a_z			# acceleration z setpoint
+bool acceleration_valid		# true if acceleration setpoint is valid/should be used
+bool acceleration_is_force	# interprete acceleration as force

msg/position_setpoint_triplet.msg

@@ -0,0 +1,8 @@
+# Global position setpoint triplet in WGS84 coordinates.
+# This are the three next waypoints (or just the next two or one).
+
+px4/position_setpoint previous
+px4/position_setpoint current
+px4/position_setpoint next
+
+uint8 nav_state	# report the navigation state

msg/templates/px4/ros/msg.h.template

@@ -0,0 +1,95 @@
+@###############################################
+@#
+@# PX4 ROS compatible message source code
+@# generation for C++
+@#
+@# This file generates the multiplatform wrapper
+@#
+@# EmPy template for generating <msg>.h files
+@# Based on the original template for ROS
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - file_name_in (String) Source file
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@#  - md5sum (String) MD5Sum of the .msg specification
+@###############################################
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/* Auto-generated by genmsg_cpp from file @file_name_in */
+
+@{
+import genmsg.msgs
+import gencpp
+
+cpp_class = 'px4_%s'%spec.short_name
+native_type = spec.short_name
+topic_name = spec.short_name
+}@
+
+#pragma once
+
+@##############################
+@# Generic Includes
+@##############################
+#include "px4/@(topic_name).h"
+#include "platforms/px4_message.h"
+
+@##############################
+@# Class
+@##############################
+
+namespace px4
+{
+
+class @(cpp_class) :
+	public PX4Message<@(native_type)>
+{
+public:
+	@(cpp_class)() :
+		PX4Message<@(native_type)>()
+	{}
+
+	@(cpp_class)(@(native_type) msg) :
+		PX4Message<@(native_type)>(msg)
+	{}
+
+	~@(cpp_class)() {}
+
+	static PX4TopicHandle handle() {return "@(topic_name)";}
+};
+
+};

msg/templates/px4/uorb/msg.h.template

@@ -0,0 +1,95 @@
+@###############################################
+@#
+@# PX4 ROS compatible message source code
+@# generation for C++
+@#
+@# This file generates the multiplatform wrapper
+@#
+@# EmPy template for generating <msg>.h files
+@# Based on the original template for ROS
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - file_name_in (String) Source file
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@#  - md5sum (String) MD5Sum of the .msg specification
+@###############################################
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/* Auto-generated by genmsg_cpp from file @file_name_in */
+
+@{
+import genmsg.msgs
+import gencpp
+
+cpp_class = 'px4_%s'%spec.short_name
+native_type = '%s_s'%spec.short_name
+topic_name = spec.short_name
+}@
+
+#pragma once
+
+@##############################
+@# Generic Includes
+@##############################
+#include <uORB/topics/@(topic_name).h>
+#include "platforms/px4_message.h"
+
+@##############################
+@# Class
+@##############################
+
+namespace px4
+{
+
+class __EXPORT @(cpp_class) :
+	public PX4Message<@(native_type)>
+{
+public:
+	@(cpp_class)() :
+		PX4Message<@(native_type)>()
+	{}
+
+	@(cpp_class)(@(native_type) msg) :
+		PX4Message<@(native_type)>(msg)
+	{}
+
+	~@(cpp_class)() {}
+
+	static PX4TopicHandle handle() {return ORB_ID(@(topic_name));}
+};
+
+};

msg/templates/uorb/msg.h.template

@@ -16,7 +16,7 @@
 @###############################################
 /****************************************************************************
  *
- *   Copyright (C) 2013-2014 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
@@ -47,17 +47,14 @@
  *
  ****************************************************************************/
 
- /* Auto-generated by genmsg_cpp from file @file_name_in */
+/* Auto-generated by genmsg_cpp from file @file_name_in */
 
 @{
 import genmsg.msgs
 import gencpp
 
-cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace
-cpp_class = '%s_'%spec.short_name
-cpp_full_name = '%s%s'%(cpp_namespace,cpp_class)
-cpp_full_name_with_alloc = '%s<ContainerAllocator>'%(cpp_full_name)
-cpp_msg_definition = gencpp.escape_message_definition(msg_definition)
+uorb_struct = '%s_s'%spec.short_name
+topic_name = spec.short_name
 }@
 
 #pragma once
@@ -66,7 +63,7 @@ cpp_msg_definition = gencpp.escape_message_definition(msg_definition)
 @# Generic Includes
 @##############################
 #include <stdint.h>
-#include "../uORB.h"
+#include <uORB/uORB.h>
 
 @##############################
 @# Includes for dependencies
@@ -80,10 +77,13 @@ for field in spec.parsed_fields():
       print('#include <uORB/topics/%s.h>'%(name))
 
 }@
-@# Constants
+
+@# Constants c style
+#ifndef __cplusplus
 @[for constant in spec.constants]@
 #define @(constant.name) @(int(constant.val))
 @[end for]
+#endif
 
 /**
  * @@addtogroup topics
@@ -104,8 +104,10 @@ type_map = {'int8': 'int8_t',
   'uint32': 'uint32_t',
   'uint64': 'uint64_t',
   'float32': 'float',
+  'float64': 'double',
   'bool': 'bool',
-  'fence_vertex': 'fence_vertex'}
+  'fence_vertex': 'fence_vertex',
+  'position_setpoint': 'position_setpoint'}
 
 # Function to print a standard ros type
 def print_field_def(field):
@@ -136,13 +138,33 @@ def print_field_def(field):
   print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
 
 }
-struct @(spec.short_name)_s {
+#ifdef __cplusplus
+@#class @(uorb_struct) {
+struct __EXPORT @(uorb_struct) {
+@#public:
+#else
+struct @(uorb_struct) {
+#endif
 @{
 # loop over all fields and print the type and name
 for field in spec.parsed_fields():
   if (not field.is_header):
     print_field_def(field)
 }@
+#ifdef __cplusplus
+@# Constants again c++-ified
+@{
+for constant in spec.constants:
+  type = constant.type
+  if type in type_map:
+    # need to add _t: int8 --> int8_t
+    type_px4 = type_map[type]
+  else:
+    raise Exception("Type {0} not supported, add to to template file!".format(type))
+
+  print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val)))
+}
+#endif
 };
 
 /**
@@ -150,4 +172,4 @@ for field in spec.parsed_fields():
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(@(spec.short_name));
+ORB_DECLARE(@(topic_name));

msg/vehicle_attitude.msg

@@ -16,20 +16,3 @@ float32[4] q		# Quaternion (NED)
 float32[3] g_comp		# Compensated gravity vector
 bool R_valid		# Rotation matrix valid
 bool q_valid		# Quaternion valid
-
-# secondary attitude for VTOL
-float32 roll_sec		# Roll angle (rad, Tait-Bryan, NED)
-float32 pitch_sec		# Pitch angle (rad, Tait-Bryan, NED)
-float32 yaw_sec			# Yaw angle (rad, Tait-Bryan, NED)
-float32 rollspeed_sec		# Roll angular speed (rad/s, Tait-Bryan, NED)
-float32 pitchspeed_sec		# Pitch angular speed (rad/s, Tait-Bryan, NED)
-float32 yawspeed_sec	    	# Yaw angular speed (rad/s, Tait-Bryan, NED)
-float32 rollacc_sec		# Roll angular accelration (rad/s, Tait-Bryan, NED)
-float32 pitchacc_sec	    	# Pitch angular acceleration (rad/s, Tait-Bryan, NED)
-float32 yawacc_sec		# Yaw angular acceleration (rad/s, Tait-Bryan, NED)
-float32[3] rate_offsets_sec	# Offsets of the body angular rates from zero
-float32[9] R_sec		# Rotation matrix, body to world, (Tait-Bryan, NED)
-float32[4] q_sec		# Quaternion (NED)
-float32[3] g_comp_sec		# Compensated gravity vector
-bool R_valid_sec		# Rotation matrix valid
-bool q_valid_sec		# Quaternion valid

msg/vehicle_global_velocity_setpoint.msg

@@ -0,0 +1,4 @@
+# Velocity setpoint in NED frame
+float32 vx		# in m/s NED
+float32 vy		# in m/s NED
+float32 vz		# in m/s NED

msg/vehicle_local_position.msg

@@ -0,0 +1,36 @@
+# Fused local position in NED.
+
+uint64 timestamp		# Time of this estimate, in microseconds since system start
+bool xy_valid			# true if x and y are valid
+bool z_valid			# true if z is valid
+bool v_xy_valid		# true if vy and vy are valid
+bool v_z_valid			# true if vz is valid
+
+# Position in local NED frame
+float32 x				# X position in meters in NED earth-fixed frame
+float32 y				# X position in meters in NED earth-fixed frame
+float32 z				# Z position in meters in NED earth-fixed frame (negative altitude)
+
+# Velocity in NED frame
+float32 vx 				# Ground X Speed (Latitude), m/s in NED
+float32 vy				# Ground Y Speed (Longitude), m/s in NED
+float32 vz				# Ground Z Speed (Altitude), m/s	in NED
+
+# Heading
+float32 yaw
+
+# Reference position in GPS / WGS84 frame
+bool xy_global			# true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
+bool z_global			# true if z is valid and has valid global reference (ref_alt)
+uint64 ref_timestamp	# Time when reference position was set
+float64 ref_lat		# Reference point latitude in degrees
+float64 ref_lon		# Reference point longitude in degrees
+float32 ref_alt			# Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level
+
+# Distance to surface
+float32 dist_bottom		# Distance to bottom surface (ground)
+float32 dist_bottom_rate		# Distance to bottom surface (ground) change rate
+uint64 surface_bottom_timestamp		# Time when new bottom surface found
+bool dist_bottom_valid	# true if distance to bottom surface is valid
+float32 eph
+float32 epv

msg/vehicle_local_position_setpoint.msg

@@ -0,0 +1,7 @@
+# Local position in NED frame
+
+uint64 timestamp	# timestamp of the setpoint
+float32 x		# in meters NED
+float32 y		# in meters NED
+float32 z		# in meters NED
+float32 yaw		# in radians NED -PI..+PI

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}"

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
 
 #

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}"

nuttx-configs/px4fmu-v1/nsh/defconfig

@@ -92,7 +92,7 @@ CONFIG_ARCH_HAVE_MPU=y
 #
 # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
 CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
-CONFIG_ARMV7M_STACKCHECK=y
+CONFIG_ARMV7M_STACKCHECK=n
 CONFIG_SERIAL_TERMIOS=y
 
 #
@@ -417,7 +417,7 @@ CONFIG_PREALLOC_TIMERS=50
 # Stack and heap information
 #
 CONFIG_IDLETHREAD_STACKSIZE=3500
-CONFIG_USERMAIN_STACKSIZE=3000
+CONFIG_USERMAIN_STACKSIZE=2600
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 
@@ -719,11 +719,11 @@ CONFIG_SCHED_WORKQUEUE=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_WORKPRIORITY=192
 CONFIG_SCHED_WORKPERIOD=5000
-CONFIG_SCHED_WORKSTACKSIZE=2048
+CONFIG_SCHED_WORKSTACKSIZE=1800
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKPERIOD=50000
-CONFIG_SCHED_LPWORKSTACKSIZE=2048
+CONFIG_SCHED_LPWORKSTACKSIZE=1800
 # CONFIG_LIB_KBDCODEC is not set
 # CONFIG_LIB_SLCDCODEC is not set
 

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}"

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -117,7 +117,7 @@ CONFIG_ARCH_HAVE_MPU=y
 #
 # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
 CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
-CONFIG_ARMV7M_STACKCHECK=y
+CONFIG_ARMV7M_STACKCHECK=n
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SDIO_DMA=y
 CONFIG_SDIO_DMAPRIO=0x00010000
@@ -451,7 +451,7 @@ CONFIG_PREALLOC_TIMERS=50
 # Stack and heap information
 #
 CONFIG_IDLETHREAD_STACKSIZE=3500
-CONFIG_USERMAIN_STACKSIZE=3000
+CONFIG_USERMAIN_STACKSIZE=2600
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 
@@ -797,11 +797,11 @@ CONFIG_SCHED_WORKQUEUE=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_WORKPRIORITY=192
 CONFIG_SCHED_WORKPERIOD=5000
-CONFIG_SCHED_WORKSTACKSIZE=2000
+CONFIG_SCHED_WORKSTACKSIZE=1800
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKPERIOD=50000
-CONFIG_SCHED_LPWORKSTACKSIZE=2000
+CONFIG_SCHED_LPWORKSTACKSIZE=1800
 # CONFIG_LIB_KBDCODEC is not set
 # CONFIG_LIB_SLCDCODEC is not set
 

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
 

nuttx-configs/px4io-v1/nsh/defconfig

@@ -93,7 +93,7 @@ CONFIG_ARCH_DMA=y
 CONFIG_ARCH_MATH_H=y
 
 CONFIG_ARMV7M_CMNVECTOR=y
-# CONFIG_ARMV7M_STACKCHECK is not set
+CONFIG_ARMV7M_STACKCHECK=n
 
 #
 # JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):

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
 

nuttx-configs/px4io-v2/nsh/defconfig

@@ -89,7 +89,7 @@ CONFIG_ARCH_DMA=y
 CONFIG_ARCH_MATH_H=y
 
 CONFIG_ARMV7M_CMNVECTOR=y
-
+CONFIG_ARMV7M_STACKCHECK=n
 #
 # JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):
 #

src/drivers/ardrone_interface/ardrone_interface.c

@@ -55,6 +55,10 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_control_mode.h>
 
@@ -98,7 +102,7 @@ usage(const char *reason)
  * The deamon app only briefly exists to start
  * the background job. The stack size assigned in the
  * Makefile does only apply to this management task.
- * 
+ *
  * The actual stack size should be set in the call
  * to task_create().
  */
@@ -319,7 +323,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 			/* get a local copy of the actuator controls */
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-			
+
 			/* for now only spin if armed and immediately shut down
 			 * if in failsafe
 			 */

src/drivers/ardrone_interface/ardrone_motor_control.c

@@ -339,7 +339,8 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
 	outputs.output[3] = motor4;
 	static orb_advert_t pub = 0;
 	if (pub == 0) {
-		pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
+		/* advertise to channel 0 / primary */
+		pub = orb_advertise(ORB_ID(actuator_outputs), &outputs);
 	}
 
 	if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
@@ -350,7 +351,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
 		fsync(ardrone_fd);
 
 		/* publish just written values */
-		orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs);
+		orb_publish(ORB_ID(actuator_outputs), pub, &outputs);
 
 		if (ret == sizeof(buf)) {
 			return OK;

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

src/drivers/blinkm/blinkm.cpp

@@ -571,7 +571,7 @@ BlinkM::led()
 				printf("<blinkm> cells found:%d\n", num_of_cells);
 
 			} else {
-				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
+				if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
 					/* LED Pattern for battery critical alerting */
 					led_color_1 = LED_RED;
 					led_color_2 = LED_RED;
@@ -595,7 +595,7 @@ BlinkM::led()
 					led_color_8 = LED_BLUE;
 					led_blink = LED_BLINK;
 
-				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+				} else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
 					/* LED Pattern for battery low warning */
 					led_color_1 = LED_YELLOW;
 					led_color_2 = LED_YELLOW;
@@ -647,14 +647,14 @@ BlinkM::led()
 
 						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
 							/* indicate main control state */
-							if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
+							if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL)
 								led_color_4 = LED_GREEN;
 							/* TODO: add other Auto modes */
-							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION)
+							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION)
 								led_color_4 = LED_BLUE;
-							else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
+							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL)
 								led_color_4 = LED_YELLOW;
-							else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
+							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL)
 								led_color_4 = LED_WHITE;
 							else
 								led_color_4 = LED_OFF;

src/drivers/drv_accel.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -81,9 +81,7 @@ struct accel_scale {
 /*
  * ObjDev tag for raw accelerometer data.
  */
-ORB_DECLARE(sensor_accel0);
-ORB_DECLARE(sensor_accel1);
-ORB_DECLARE(sensor_accel2);
+ORB_DECLARE(sensor_accel);
 
 /*
  * ioctl() definitions

src/drivers/drv_baro.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -63,9 +63,7 @@ struct baro_report {
 /*
  * ObjDev tag for raw barometer data.
  */
-ORB_DECLARE(sensor_baro0);
-ORB_DECLARE(sensor_baro1);
-ORB_DECLARE(sensor_baro2);
+ORB_DECLARE(sensor_baro);
 
 /*
  * ioctl() definitions

src/drivers/drv_blinkm.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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

src/drivers/drv_gyro.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -81,9 +81,7 @@ struct gyro_scale {
 /*
  * ObjDev tag for raw gyro data.
  */
-ORB_DECLARE(sensor_gyro0);
-ORB_DECLARE(sensor_gyro1);
-ORB_DECLARE(sensor_gyro2);
+ORB_DECLARE(sensor_gyro);
 
 /*
  * ioctl() definitions

src/drivers/drv_mag.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -44,6 +44,7 @@
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
 
+
 #define MAG_DEVICE_PATH		"/dev/mag"
 
 /**
@@ -79,15 +80,8 @@ struct mag_scale {
 /*
  * ObjDev tag for raw magnetometer data.
  */
-ORB_DECLARE(sensor_mag0);
-ORB_DECLARE(sensor_mag1);
-ORB_DECLARE(sensor_mag2);
+ORB_DECLARE(sensor_mag);
 
-/*
- * mag device types, for _device_id
- */
-#define DRV_MAG_DEVTYPE_HMC5883 1
-#define DRV_MAG_DEVTYPE_LSM303D 2
 
 /*
  * ioctl() definitions

src/drivers/drv_orb_dev.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -35,7 +35,9 @@
 #define _DRV_UORB_H
 
 /**
- * @file uORB published object driver.
+ * @file drv_orb_dev.h
+ * 
+ * uORB published object driver.
  */
 
 #include <sys/types.h>
@@ -84,4 +86,7 @@
 /** Get the global advertiser handle for the topic */
 #define ORBIOCGADVERTISER	_ORBIOC(13)
 
+/** Get the priority for the topic */
+#define ORBIOCGPRIORITY		_ORBIOC(14)
+
 #endif /* _DRV_UORB_H */

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

src/drivers/drv_sensor.h

@@ -43,6 +43,24 @@
 #include <stdint.h>
 #include <sys/ioctl.h>
 
+#include "drv_device.h"
+
+/**
+ * Sensor type definitions.
+ *
+ * Used to create a unique device id for redundant and combo sensors
+ */
+
+#define DRV_MAG_DEVTYPE_HMC5883 0x01
+#define DRV_MAG_DEVTYPE_LSM303D 0x02
+#define DRV_ACC_DEVTYPE_LSM303D 0x11
+#define DRV_ACC_DEVTYPE_BMA180  0x12
+#define DRV_ACC_DEVTYPE_MPU6000 0x13
+#define DRV_GYR_DEVTYPE_MPU6000 0x21
+#define DRV_GYR_DEVTYPE_L3GD20  0x22
+#define DRV_RNG_DEVTYPE_MB12XX  0x31
+#define DRV_RNG_DEVTYPE_LL40LS  0x32
+
 /*
  * ioctl() definitions
  *

src/drivers/gps/gps_helper.cpp

@@ -88,8 +88,6 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 
 	case 115200: speed = B115200; break;
 
-		warnx("try baudrate: %d\n", speed);
-
 	default:
 		warnx("ERR: baudrate: %d\n", baud);
 		return -EINVAL;

src/drivers/hil/hil.cpp

@@ -75,6 +75,8 @@
 #include <systemlib/mixer/mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 
@@ -250,7 +252,7 @@ HIL::task_main_trampoline(int argc, char *argv[])
 int
 HIL::set_mode(Mode mode)
 {
-	/* 
+	/*
 	 * Configure for PWM output.
 	 *
 	 * Note that regardless of the configured mode, the task is always
@@ -269,19 +271,19 @@ HIL::set_mode(Mode mode)
 		/* multi-port as 4 PWM outs */
 		_update_rate = 50;	/* default output rate */
 		break;
-            
+
     	case MODE_8PWM:
             debug("MODE_8PWM");
             /* multi-port as 8 PWM outs */
             _update_rate = 50;	/* default output rate */
             break;
-            
+
         case MODE_12PWM:
             debug("MODE_12PWM");
             /* multi-port as 12 PWM outs */
             _update_rate = 50;	/* default output rate */
             break;
-            
+
         case MODE_16PWM:
             debug("MODE_16PWM");
             /* multi-port as 16 PWM outs */
@@ -330,8 +332,9 @@ HIL::task_main()
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
 	/* advertise the mixed control outputs */
-	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
-				   &outputs);
+	int dummy;
+	_t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
+				   &outputs, &dummy, ORB_PRIO_LOW);
 
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
@@ -423,7 +426,7 @@ HIL::task_main()
 				}
 
 				/* and publish for anyone that cares to see */
-				orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
+				orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs);
 			}
 		}
 
@@ -513,12 +516,12 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET_UPDATE_RATE:
-		// HIL always outputs at the alternate (usually faster) rate 
+		// HIL always outputs at the alternate (usually faster) rate
 		g_hil->set_pwm_rate(arg);
 		break;
 
 	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
-		// HIL always outputs at the alternate (usually faster) rate 
+		// HIL always outputs at the alternate (usually faster) rate
 		break;
 
 	case PWM_SERVO_SET(2):
@@ -659,7 +662,7 @@ int
 hil_new_mode(PortMode new_mode)
 {
 	// uint32_t gpio_bits;
-	
+
 
 //	/* reset to all-inputs */
 //	g_hil->ioctl(0, GPIO_RESET, 0);
@@ -691,17 +694,17 @@ hil_new_mode(PortMode new_mode)
 		/* select 2-pin PWM mode */
 		servo_mode = HIL::MODE_2PWM;
 		break;
-            
+
         case PORT2_8PWM:
             /* select 8-pin PWM mode */
             servo_mode = HIL::MODE_8PWM;
             break;
-            
+
         case PORT2_12PWM:
             /* select 12-pin PWM mode */
             servo_mode = HIL::MODE_12PWM;
             break;
-            
+
         case PORT2_16PWM:
             /* select 16-pin PWM mode */
             servo_mode = HIL::MODE_16PWM;

src/drivers/hmc5883/hmc5883.cpp

@@ -66,9 +66,9 @@
 #include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/device/ringbuffer.h>
+#include <drivers/drv_device.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 
 #include <float.h>
 #include <getopt.h>
@@ -156,10 +156,9 @@ private:
 	float 			_range_ga;
 	bool			_collect_phase;
 	int			_class_instance;
+	int			_orb_class_instance;
 
 	orb_advert_t		_mag_topic;
-	orb_advert_t		_subsystem_pub;
-	orb_id_t		_mag_orb_id;
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
@@ -347,9 +346,8 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota
 	_range_ga(1.3f),
 	_collect_phase(false),
 	_class_instance(-1),
+	_orb_class_instance(-1),
 	_mag_topic(-1),
-	_subsystem_pub(-1),
-	_mag_orb_id(nullptr),
 	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@@ -418,7 +416,6 @@ HMC5883::init()
 	reset();
 
 	_class_instance = register_class_devname(MAG_DEVICE_PATH);
-	_mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance);
 
 	ret = OK;
 	/* sensor is ok, but not calibrated */
@@ -725,6 +722,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 		debug("MAGIOCGEXTERNAL in main driver");
 		return _interface->ioctl(cmd, dummy);
 
+	case DEVIOCGDEVICEID:
+		return _interface->ioctl(cmd, dummy);
+
 	default:
 		/* give it to the superclass */
 		return CDev::ioctl(filp, cmd, arg);
@@ -846,6 +846,7 @@ HMC5883::collect()
 
 	perf_begin(_sample_perf);
 	struct mag_report new_report;
+	bool sensor_is_onboard = false;
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	new_report.timestamp = hrt_absolute_time();
@@ -898,7 +899,8 @@ HMC5883::collect()
 
 	// XXX revisit for SPI part, might require a bus type IOCTL
 	unsigned dummy;
-	if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) {
+	sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
+	if (sensor_is_onboard) {
 		// convert onboard so it matches offboard for the
 		// scaling below
 		report.y = -report.y;
@@ -921,9 +923,10 @@ HMC5883::collect()
 
 		if (_mag_topic != -1) {
 			/* publish it */
-			orb_publish(_mag_orb_id, _mag_topic, &new_report);
+			orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
 		} else {
-			_mag_topic = orb_advertise(_mag_orb_id, &new_report);
+			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
+				&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
 
 			if (_mag_topic < 0)
 				debug("ADVERT FAIL");
@@ -1181,24 +1184,6 @@ int HMC5883::check_calibration()
 		warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
 					  (offset_valid) ? "" : "offset invalid");
 		_calibrated = (offset_valid && scale_valid);
-
-
-		// XXX Change advertisement
-
-		/* notify about state change */
-		struct subsystem_info_s info = {
-			true,
-			true,
-			_calibrated,
-			SUBSYSTEM_TYPE_MAG};
-
-		if (!(_pub_blocked)) {
-			if (_subsystem_pub > 0) {
-				orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
-			} else {
-				_subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
-			}
-		}
 	}
 
 	/* return 0 if calibrated, 1 else */
@@ -1305,9 +1290,9 @@ struct hmc5883_bus_option {
 	uint8_t busnum;
 	HMC5883	*dev;
 } bus_options[] = {
-	{ HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
+	{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
 #ifdef PX4_I2C_BUS_ONBOARD
-	{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
+	{ HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
 #endif
 #ifdef PX4_SPIDEV_HMC
 	{ HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
@@ -1347,13 +1332,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;
 }

src/drivers/hmc5883/hmc5883_i2c.cpp

@@ -53,6 +53,7 @@
 
 #include <drivers/device/i2c.h>
 #include <drivers/drv_mag.h>
+#include <drivers/drv_device.h>
 
 #include "hmc5883.h"
 
@@ -90,6 +91,7 @@ HMC5883_I2C_interface(int bus)
 HMC5883_I2C::HMC5883_I2C(int bus) :
 	I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000)
 {
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
 }
 
 HMC5883_I2C::~HMC5883_I2C()
@@ -117,6 +119,9 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg)
 			return 0;
 		}
 
+	case DEVIOCGDEVICEID:
+		return CDev::ioctl(nullptr, operation, arg);
+
 	default:
 		ret = -EINVAL;
 	}

src/drivers/hmc5883/hmc5883_spi.cpp

@@ -53,6 +53,7 @@
 
 #include <drivers/device/spi.h>
 #include <drivers/drv_mag.h>
+#include <drivers/drv_device.h>
 
 #include "hmc5883.h"
 #include <board_config.h>
@@ -92,6 +93,7 @@ HMC5883_SPI_interface(int bus)
 HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) :
 	SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */)
 {
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
 }
 
 HMC5883_SPI::~HMC5883_SPI()
@@ -146,6 +148,9 @@ HMC5883_SPI::ioctl(unsigned operation, unsigned &arg)
 			return 0;
 		}
 
+	case DEVIOCGDEVICEID:
+		return CDev::ioctl(nullptr, operation, arg);
+
 	default:
 	{
 		ret = -EINVAL;

src/drivers/l3gd20/l3gd20.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -33,7 +33,7 @@
 
 /**
  * @file l3gd20.cpp
- * Driver for the ST L3GD20 MEMS gyro connected via SPI.
+ * Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI.
  *
  * Note: With the exception of the self-test feature, the ST L3G4200D is
  *       also supported by this driver.
@@ -179,6 +179,12 @@ static const int ERROR = -1;
 #define L3GD20_DEFAULT_FILTER_FREQ		30
 #define L3GD20_TEMP_OFFSET_CELSIUS		40
 
+#ifdef PX4_SPI_BUS_EXT
+#define EXTERNAL_BUS PX4_SPI_BUS_EXT
+#else
+#define EXTERNAL_BUS 0
+#endif
+
 #ifndef SENSOR_BOARD_ROTATION_DEFAULT
 #define SENSOR_BOARD_ROTATION_DEFAULT		SENSOR_BOARD_ROTATION_270_DEG
 #endif
@@ -214,14 +220,14 @@ private:
 
 	struct hrt_call		_call;
 	unsigned		_call_interval;
-	
+
 	RingBuffer		*_reports;
 
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
 	orb_advert_t		_gyro_topic;
-	orb_id_t		_orb_id;
+	int			_orb_class_instance;
 	int			_class_instance;
 
 	unsigned		_current_rate;
@@ -273,6 +279,13 @@ private:
 	 */
 	void			disable_i2c();
 
+	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_bus == EXTERNAL_BUS); }
+
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
@@ -391,7 +404,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
-	_orb_id(nullptr),
+	_orb_class_instance(-1),
 	_class_instance(-1),
 	_current_rate(0),
 	_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
@@ -411,6 +424,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
 	// enable debug() calls
 	_debug_enabled = true;
 
+	_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_L3GD20;
+
 	// default scale factors
 	_gyro_scale.x_offset = 0;
 	_gyro_scale.x_scale  = 1.0f;
@@ -456,20 +471,6 @@ L3GD20::init()
 
 	_class_instance = register_class_devname(GYRO_DEVICE_PATH);
 
-	switch (_class_instance) {
-		case CLASS_DEVICE_PRIMARY:
-			_orb_id = ORB_ID(sensor_gyro0);
-			break;
-
-		case CLASS_DEVICE_SECONDARY:
-			_orb_id = ORB_ID(sensor_gyro1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_orb_id = ORB_ID(sensor_gyro2);
-			break;
-	}
-
 	reset();
 
 	measure();
@@ -478,7 +479,8 @@ L3GD20::init()
 	struct gyro_report grp;
 	_reports->get(&grp);
 
-	_gyro_topic = orb_advertise(_orb_id, &grp);
+	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
+		&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
 
 	if (_gyro_topic < 0) {
 		debug("failed to create sensor_gyro publication");
@@ -639,7 +641,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 			return -ENOMEM;
 		}
 		irqrestore(flags);
-		
+
 		return OK;
 	}
 
@@ -867,7 +869,7 @@ L3GD20::reset()
 	disable_i2c();
 
 	/* set default configuration */
-	write_checked_reg(ADDR_CTRL_REG1, 
+	write_checked_reg(ADDR_CTRL_REG1,
                           REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
 	write_checked_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
 	write_checked_reg(ADDR_CTRL_REG3, 0x08);        /* DRDY enable */
@@ -911,7 +913,7 @@ L3GD20::check_registers(void)
 		  if we get the wrong value then we know the SPI bus
 		  or sensor is very sick. We set _register_wait to 20
 		  and wait until we have seen 20 good values in a row
-		  before we consider the sensor to be OK again. 
+		  before we consider the sensor to be OK again.
 		 */
 		perf_count(_bad_registers);
 
@@ -974,7 +976,7 @@ L3GD20::measure()
               we waited for DRDY, but did not see DRDY on all axes
               when we captured. That means a transfer error of some sort
              */
-            perf_count(_errors);            
+            perf_count(_errors);
             return;
         }
 #endif
@@ -994,7 +996,7 @@ L3GD20::measure()
 	 */
 	report.timestamp = hrt_absolute_time();
         report.error_count = perf_event_count(_bad_registers);
-	
+
 	switch (_orientation) {
 
 		case SENSOR_BOARD_ROTATION_000_DEG:
@@ -1050,7 +1052,7 @@ L3GD20::measure()
 	/* publish for subscribers */
 	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(_orb_id, _gyro_topic, &report);
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
 	}
 
 	_read++;
@@ -1072,7 +1074,7 @@ L3GD20::print_info()
         for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
             uint8_t v = read_reg(_checked_registers[i]);
             if (v != _checked_values[i]) {
-                ::printf("reg %02x:%02x should be %02x\n", 
+                ::printf("reg %02x:%02x should be %02x\n",
                          (unsigned)_checked_registers[i],
                          (unsigned)v,
                          (unsigned)_checked_values[i]);

src/drivers/lsm303d/lsm303d.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 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
@@ -211,6 +211,12 @@ static const int ERROR = -1;
 
 #define LSM303D_ONE_G					9.80665f
 
+#ifdef PX4_SPI_BUS_EXT
+#define EXTERNAL_BUS PX4_SPI_BUS_EXT
+#else
+#define EXTERNAL_BUS 0
+#endif
+
 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
 
 
@@ -275,7 +281,7 @@ private:
 	unsigned		_mag_samplerate;
 
 	orb_advert_t		_accel_topic;
-	orb_id_t		_accel_orb_id;
+	int			_accel_orb_class_instance;
 	int			_accel_class_instance;
 
 	unsigned		_accel_read;
@@ -295,7 +301,7 @@ private:
 
 	enum Rotation		_rotation;
 
-	// values used to 
+	// values used to
 	float			_last_accel[3];
 	uint8_t			_constant_accel_count;
 
@@ -329,6 +335,13 @@ private:
 	 */
 	void			disable_i2c();
 
+	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_bus == EXTERNAL_BUS); }
+
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
@@ -507,7 +520,7 @@ private:
 	LSM303D				*_parent;
 
 	orb_advert_t			_mag_topic;
-	orb_id_t			_mag_orb_id;
+	int				_mag_orb_class_instance;
 	int				_mag_class_instance;
 
 	void				measure();
@@ -539,7 +552,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_mag_range_scale(0.0f),
 	_mag_samplerate(0),
 	_accel_topic(-1),
-	_accel_orb_id(nullptr),
+	_accel_orb_class_instance(-1),
 	_accel_class_instance(-1),
 	_accel_read(0),
 	_mag_read(0),
@@ -556,11 +569,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_constant_accel_count(0),
 	_checked_next(0)
 {
-	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
+
 
 	// enable debug() calls
 	_debug_enabled = true;
 
+	_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D;
+	
+	/* Prime _mag with parents devid. */
+	_mag->_device_id.devid = _device_id.devid;
+	_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
+
+
 	// default scale factors
 	_accel_scale.x_offset = 0.0f;
 	_accel_scale.x_scale  = 1.0f;
@@ -618,7 +638,6 @@ LSM303D::init()
 	if (_accel_reports == nullptr)
 		goto out;
 
-	/* advertise accel topic */
 	_mag_reports = new RingBuffer(2, sizeof(mag_report));
 
 	if (_mag_reports == nullptr)
@@ -641,26 +660,14 @@ LSM303D::init()
 	_mag_reports->get(&mrp);
 
 	/* measurement will have generated a report, publish */
-	switch (_mag->_mag_class_instance) {
-		case CLASS_DEVICE_PRIMARY:
-			_mag->_mag_orb_id = ORB_ID(sensor_mag0);
-			break;
-
-		case CLASS_DEVICE_SECONDARY:
-			_mag->_mag_orb_id = ORB_ID(sensor_mag1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_mag->_mag_orb_id = ORB_ID(sensor_mag2);
-			break;
-	}
-
-	_mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp);
+	_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
+		&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
 
 	if (_mag->_mag_topic < 0) {
 		warnx("ADVERT ERR");
 	}
 
+
 	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
 
 	/* advertise sensor topic, measure manually to initialize valid report */
@@ -668,21 +675,8 @@ LSM303D::init()
 	_accel_reports->get(&arp);
 
 	/* measurement will have generated a report, publish */
-	switch (_accel_class_instance) {
-		case CLASS_DEVICE_PRIMARY:
-			_accel_orb_id = ORB_ID(sensor_accel0);
-			break;
-
-		case CLASS_DEVICE_SECONDARY:
-			_accel_orb_id = ORB_ID(sensor_accel1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_accel_orb_id = ORB_ID(sensor_accel2);
-			break;
-	}
-
-	_accel_topic = orb_advertise(_accel_orb_id, &arp);
+	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
+		&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
 
 	if (_accel_topic < 0) {
 		warnx("ADVERT ERR");
@@ -712,7 +706,7 @@ LSM303D::reset()
 	disable_i2c();
 
 	/* enable accel*/
-	write_checked_reg(ADDR_CTRL_REG1, 
+	write_checked_reg(ADDR_CTRL_REG1,
 			  REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A);
 
 	/* enable mag */
@@ -746,7 +740,7 @@ LSM303D::probe()
 
 	/* verify that the device is attached and functioning */
 	bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
-	
+
 	if (success) {
 		_checked_values[0] = WHO_I_AM;
 		return OK;
@@ -1019,7 +1013,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 			return SENSOR_POLLRATE_MANUAL;
 
 		return 1000000 / _call_mag_interval;
-	
+
 	case SENSORIOCSQUEUEDEPTH: {
 		/* lower bound is mandatory, upper bound is a sanity check */
 		if ((arg < 1) || (arg > 100))
@@ -1424,7 +1418,7 @@ LSM303D::check_registers(void)
 		  if we get the wrong value then we know the SPI bus
 		  or sensor is very sick. We set _register_wait to 20
 		  and wait until we have seen 20 good values in a row
-		  before we consider the sensor to be OK again. 
+		  before we consider the sensor to be OK again.
 		 */
 		perf_count(_bad_registers);
 
@@ -1548,7 +1542,7 @@ LSM303D::measure()
 		perf_count(_bad_values);
 		_constant_accel_count = 0;
 	}
-	    
+
 	_last_accel[0] = x_in_new;
 	_last_accel[1] = y_in_new;
 	_last_accel[2] = z_in_new;
@@ -1570,7 +1564,7 @@ LSM303D::measure()
 
 	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(_accel_orb_id, _accel_topic, &accel_report);
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 	}
 
 	_accel_read++;
@@ -1641,7 +1635,7 @@ LSM303D::mag_measure()
 
 	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report);
+		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
 	}
 
 	_mag_read++;
@@ -1666,7 +1660,7 @@ LSM303D::print_info()
         for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) {
             uint8_t v = read_reg(_checked_registers[i]);
             if (v != _checked_values[i]) {
-                ::printf("reg %02x:%02x should be %02x\n", 
+                ::printf("reg %02x:%02x should be %02x\n",
                          (unsigned)_checked_registers[i],
                          (unsigned)v,
                          (unsigned)_checked_values[i]);
@@ -1742,7 +1736,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) :
 	CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
 	_parent(parent),
 	_mag_topic(-1),
-	_mag_orb_id(nullptr),
+	_mag_orb_class_instance(-1),
 	_mag_class_instance(-1)
 {
 }
@@ -1783,7 +1777,13 @@ LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
 int
 LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
-	return _parent->mag_ioctl(filp, cmd, arg);
+	switch (cmd) {
+		case DEVIOCGDEVICEID:
+			return (int)CDev::ioctl(filp, cmd, arg);
+			break;
+		default:
+			return _parent->mag_ioctl(filp, cmd, arg);
+	}
 }
 
 void

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[])

src/drivers/mkblctrl/mkblctrl.cpp

@@ -74,6 +74,7 @@
 #include <drivers/drv_mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/esc_status.h>
@@ -456,8 +457,9 @@ MK::task_main()
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
 	/* advertise the mixed control outputs */
-	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
-				   &outputs);
+	int dummy;
+	_t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
+				   &outputs, &dummy, ORB_PRIO_HIGH);
 
 	/* advertise the blctrl status */
 	esc_status_s esc;

src/drivers/mpu6000/mpu6000.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -175,6 +175,12 @@
 
 #define MPU6000_ONE_G					9.80665f
 
+#ifdef PX4_SPI_BUS_EXT
+#define EXTERNAL_BUS PX4_SPI_BUS_EXT
+#else
+#define EXTERNAL_BUS 0
+#endif
+
 /*
   the MPU6000 can only handle high SPI bus speeds on the sensor and
   interrupt status registers. All other registers have a maximum 1MHz
@@ -234,7 +240,7 @@ private:
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
-	orb_id_t		_accel_orb_id;
+	int			_accel_orb_class_instance;
 	int			_accel_class_instance;
 
 	RingBuffer		*_gyro_reports;
@@ -360,6 +366,13 @@ private:
 	 */
 	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
 
+	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_bus == EXTERNAL_BUS); }
+
 	/**
 	 * Measurement self test
 	 *
@@ -433,7 +446,7 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU
 									     MPUREG_INT_ENABLE,
 									     MPUREG_INT_PIN_CFG };
 
-					   
+
 
 /**
  * Helper class implementing the gyro driver node.
@@ -457,7 +470,7 @@ protected:
 private:
 	MPU6000			*_parent;
 	orb_advert_t		_gyro_topic;
-	orb_id_t		_gyro_orb_id;
+	int			_gyro_orb_class_instance;
 	int			_gyro_class_instance;
 
 	/* do not allow to copy this class due to pointer data members */
@@ -479,7 +492,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
-	_accel_orb_id(nullptr),
+	_accel_orb_class_instance(-1),
 	_accel_class_instance(-1),
 	_gyro_reports(nullptr),
 	_gyro_scale{},
@@ -510,6 +523,12 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	// disable debug() calls
 	_debug_enabled = false;
 
+	_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000;
+
+	/* Prime _gyro with parents devid. */
+	_gyro->_device_id.devid = _device_id.devid;
+	_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000;
+
 	// default accel scale factors
 	_accel_scale.x_offset = 0;
 	_accel_scale.x_scale  = 1.0f;
@@ -596,6 +615,7 @@ MPU6000::init()
 	_gyro_scale.z_offset = 0;
 	_gyro_scale.z_scale  = 1.0f;
 
+
 	/* do CDev init for the gyro device node, keep it optional */
 	ret = _gyro->init();
 	/* if probe/setup failed, bail now */
@@ -613,22 +633,8 @@ MPU6000::init()
 	_accel_reports->get(&arp);
 
 	/* measurement will have generated a report, publish */
-	switch (_accel_class_instance) {
-		case CLASS_DEVICE_PRIMARY:
-			_accel_orb_id = ORB_ID(sensor_accel0);
-			break;
-		
-		case CLASS_DEVICE_SECONDARY:
-			_accel_orb_id = ORB_ID(sensor_accel1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_accel_orb_id = ORB_ID(sensor_accel2);
-			break;
-
-	}
-
-	_accel_topic = orb_advertise(_accel_orb_id, &arp);
+	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
+		&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
 
 	if (_accel_topic < 0) {
 		warnx("ADVERT FAIL");
@@ -639,22 +645,8 @@ MPU6000::init()
 	struct gyro_report grp;
 	_gyro_reports->get(&grp);
 
-	switch (_gyro->_gyro_class_instance) {
-		case CLASS_DEVICE_PRIMARY:
-			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro0);
-			break;
-
-		case CLASS_DEVICE_SECONDARY:
-			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro2);
-			break;
-
-	}
-
-	_gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp);
+	_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
+		&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
 
 	if (_gyro->_gyro_topic < 0) {
 		warnx("ADVERT FAIL");
@@ -683,7 +675,7 @@ int MPU6000::reset()
 		// for it to come out of sleep
 		write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
 		up_udelay(1000);
-		
+
 		// Disable I2C bus (recommended on datasheet)
 		write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
 		irqrestore(state);
@@ -741,7 +733,7 @@ int MPU6000::reset()
 	case MPU6000_REV_D9:
 	case MPU6000_REV_D10:
 	// default case to cope with new chip revisions, which
-	// presumably won't have the accel scaling bug		
+	// presumably won't have the accel scaling bug
 	default:
 		// Accel scale 8g (4096 LSB/g)
 		write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
@@ -819,7 +811,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
 {
 	uint8_t filter;
 
-	/* 
+	/*
 	   choose next highest filter frequency available
 	 */
         if (frequency_hz == 0) {
@@ -921,26 +913,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)
-		return 1;
-	if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
+	/*
+	 * 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.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
-		return 1;
-	if (fabsf(_gyro_scale.y_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.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
+	if (fabsf(_gyro_scale.y_offset) > max_offset)
 		return 1;
-	if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
+	if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale)
 		return 1;
 
+	if (fabsf(_gyro_scale.z_offset) > max_offset)
+		return 1;
+	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%
@@ -958,7 +974,7 @@ MPU6000::factory_self_test()
 	// gyro self test has to be done at 250DPS
 	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS);
 
-	struct MPUReport mpu_report;	
+	struct MPUReport mpu_report;
 	float accel_baseline[3];
 	float gyro_baseline[3];
 	float accel[3];
@@ -988,10 +1004,10 @@ MPU6000::factory_self_test()
 	}
 
 #if 1
-	write_reg(MPUREG_GYRO_CONFIG, 
-		  BITS_FS_250DPS | 
-		  BITS_GYRO_ST_X | 
-		  BITS_GYRO_ST_Y | 
+	write_reg(MPUREG_GYRO_CONFIG,
+		  BITS_FS_250DPS |
+		  BITS_GYRO_ST_X |
+		  BITS_GYRO_ST_Y |
 		  BITS_GYRO_ST_Z);
 
 	// accel 8g, self-test enabled all axes
@@ -1061,7 +1077,7 @@ MPU6000::factory_self_test()
 			::printf("FAIL\n");
 			ret = -EIO;
 		}
-	}	
+	}
 	for (uint8_t i=0; i<3; i++) {
 		float diff = gyro[i]-gyro_baseline[i];
 		float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i];
@@ -1076,7 +1092,7 @@ MPU6000::factory_self_test()
 			::printf("FAIL\n");
 			ret = -EIO;
 		}
-	}	
+	}
 
 	write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config);
 	write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config);
@@ -1223,14 +1239,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* lower bound is mandatory, upper bound is a sanity check */
 		if ((arg < 1) || (arg > 100))
 			return -EINVAL;
-		
+
 		irqstate_t flags = irqsave();
 		if (!_accel_reports->resize(arg)) {
 			irqrestore(flags);
 			return -ENOMEM;
 		}
 		irqrestore(flags);
-		
+
 		return OK;
 	}
 
@@ -1512,13 +1528,13 @@ MPU6000::check_registers(void)
 	  the data registers.
 	*/
 	uint8_t v;
-	if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != 
+	if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
 	    _checked_values[_checked_next]) {
 		/*
 		  if we get the wrong value then we know the SPI bus
 		  or sensor is very sick. We set _register_wait to 20
 		  and wait until we have seen 20 good values in a row
-		  before we consider the sensor to be OK again. 
+		  before we consider the sensor to be OK again.
 		 */
 		perf_count(_bad_registers);
 
@@ -1631,7 +1647,7 @@ MPU6000::measure()
 		_register_wait--;
 		return;
 	}
-	    
+
 
 	/*
 	 * Swap axes and negate y
@@ -1692,7 +1708,7 @@ MPU6000::measure()
 	float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
 	float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
 	float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-	
+
 	arb.x = _accel_filter_x.apply(x_in_new);
 	arb.y = _accel_filter_y.apply(y_in_new);
 	arb.z = _accel_filter_z.apply(z_in_new);
@@ -1713,7 +1729,7 @@ MPU6000::measure()
 	float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
 	float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
 	float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
-	
+
 	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
 	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
 	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
@@ -1739,12 +1755,12 @@ MPU6000::measure()
 		perf_begin(_controller_latency_perf);
 		perf_begin(_system_latency_perf);
 		/* publish it */
-		orb_publish(_accel_orb_id, _accel_topic, &arb);
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	}
 
 	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb);
+		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
 	}
 
 	/* stop measuring */
@@ -1767,7 +1783,7 @@ MPU6000::print_info()
         for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) {
             uint8_t v = read_reg(_checked_registers[i], MPU6000_HIGH_BUS_SPEED);
             if (v != _checked_values[i]) {
-                ::printf("reg %02x:%02x should be %02x\n", 
+                ::printf("reg %02x:%02x should be %02x\n",
                          (unsigned)_checked_registers[i],
                          (unsigned)v,
                          (unsigned)_checked_values[i]);
@@ -1794,7 +1810,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
 	CDev("MPU6000_gyro", path),
 	_parent(parent),
 	_gyro_topic(-1),
-	_gyro_orb_id(nullptr),
+	_gyro_orb_class_instance(-1),
 	_gyro_class_instance(-1)
 {
 }
@@ -1839,7 +1855,14 @@ MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen)
 int
 MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
-	return _parent->gyro_ioctl(filp, cmd, arg);
+
+	switch (cmd) {
+		case DEVIOCGDEVICEID:
+			return (int)CDev::ioctl(filp, cmd, arg);
+			break;
+		default:
+			return _parent->gyro_ioctl(filp, cmd, arg);
+	}
 }
 
 /**

src/drivers/ms5611/ms5611.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -57,6 +57,7 @@
 #include <nuttx/clock.h>
 
 #include <arch/board/board.h>
+#include <board_config.h>
 
 #include <drivers/device/device.h>
 #include <drivers/drv_baro.h>
@@ -134,8 +135,7 @@ protected:
 	unsigned		_msl_pressure;	/* in Pa */
 
 	orb_advert_t		_baro_topic;
-	orb_id_t		_orb_id;
-
+	int			_orb_class_instance;
 	int			_class_instance;
 
 	perf_counter_t		_sample_perf;
@@ -171,6 +171,13 @@ protected:
 	 */
 	void			cycle();
 
+	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ }
+
 	/**
 	 * Static trampoline from the workq context; because we don't have a
 	 * generic workq wrapper yet.
@@ -210,6 +217,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char*
 	_SENS(0),
 	_msl_pressure(101325),
 	_baro_topic(-1),
+	_orb_class_instance(-1),
 	_class_instance(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
 	_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
@@ -263,7 +271,6 @@ MS5611::init()
 
 	/* register alternate interfaces if we have to */
 	_class_instance = register_class_devname(BARO_DEVICE_PATH);
-	_orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance);
 
 	struct baro_report brp;
 	/* do a first measurement cycle to populate reports with valid data */
@@ -303,7 +310,9 @@ MS5611::init()
 
 		ret = OK;
 
-		_baro_topic = orb_advertise(_orb_id, &brp);
+		_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
+				&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
+
 
 		if (_baro_topic < 0) {
 			warnx("failed to create sensor_baro publication");
@@ -725,7 +734,7 @@ MS5611::collect()
 		/* publish it */
 		if (!(_pub_blocked)) {
 			/* publish it */
-			orb_publish(_orb_id, _baro_topic, &report);
+			orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
 		}
 
 		if (_reports->force(&report)) {

src/drivers/ms5611/ms5611.h

@@ -37,12 +37,12 @@
  * Shared defines for the ms5611 driver.
  */
 
-#define ADDR_RESET_CMD			0x1E	/* write to this address to reset chip */
-#define ADDR_CMD_CONVERT_D1		0x48	/* write to this address to start temperature conversion */
-#define ADDR_CMD_CONVERT_D2		0x58	/* write to this address to start pressure conversion */
-#define ADDR_DATA			0x00	/* address of 3 bytes / 32bit pressure data */
-#define ADDR_PROM_SETUP			0xA0	/* address of 8x 2 bytes factory and calibration data */
-#define ADDR_PROM_C1			0xA2	/* address of 6x 2 bytes calibration data */
+#define ADDR_RESET_CMD				0x1E	/* write to this address to reset chip */
+#define ADDR_CMD_CONVERT_D1		0x48	/* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D2		0x58	/* write to this address to start temperature conversion */
+#define ADDR_DATA							0x00	/* address of 3 bytes / 32bit pressure data */
+#define ADDR_PROM_SETUP				0xA0	/* address of 8x 2 bytes factory and calibration data */
+#define ADDR_PROM_C1					0xA2	/* address of 6x 2 bytes calibration data */
 
 /* interface ioctls */
 #define IOCTL_RESET			2

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

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;
-};
+#include "i2c_frame.h"
+
 struct i2c_frame f;
-
-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_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);
 

src/drivers/px4fmu/fmu.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-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
@@ -70,6 +70,10 @@
 #include <drivers/drv_rc_input.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 
@@ -138,11 +142,11 @@ private:
 
 	uint32_t	_groups_required;
 	uint32_t	_groups_subscribed;
-	int		_control_subs[NUM_ACTUATOR_CONTROL_GROUPS];
-	actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS];
-	orb_id_t	_control_topics[NUM_ACTUATOR_CONTROL_GROUPS];
-	orb_id_t	_actuator_output_topic;
-	pollfd	_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS];
+	int		_control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+	actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+	orb_id_t	_control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+	int		_actuator_output_topic_instance;
+	pollfd	_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	unsigned	_poll_fds_num;
 
 	pwm_limit_t	_pwm_limit;
@@ -256,7 +260,7 @@ PX4FMU::PX4FMU() :
 	_groups_required(0),
 	_groups_subscribed(0),
 	_control_subs{-1},
-	_actuator_output_topic(nullptr),
+	_actuator_output_topic_instance(-1),
 	_poll_fds_num(0),
 	_pwm_limit{},
 	_failsafe_pwm{0},
@@ -327,8 +331,6 @@ PX4FMU::init()
 		log("default PWM output device");
 	}
 
-	_actuator_output_topic = ORB_ID_DOUBLE(actuator_outputs_, _class_instance);
-
 	/* reset GPIOs */
 	gpio_reset();
 
@@ -510,7 +512,7 @@ PX4FMU::subscribe()
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
 	_poll_fds_num = 0;
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
 			_control_subs[i] = orb_subscribe(_control_topics[i]);
@@ -587,7 +589,7 @@ PX4FMU::task_main()
 			}
 
 			debug("adjusted actuator update interval to %ums", update_rate_in_ms);
-			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					orb_set_interval(_control_subs[i], update_rate_in_ms);
 				}
@@ -614,7 +616,7 @@ PX4FMU::task_main()
 
 			/* get controls for required topics */
 			unsigned poll_id = 0;
-			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					if (_poll_fds[poll_id].revents & POLLIN) {
 						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
@@ -679,10 +681,10 @@ PX4FMU::task_main()
 
 				/* publish mixed control outputs */
 				if (_outputs_pub < 0) {
-					_outputs_pub = orb_advertise(_actuator_output_topic, &outputs);
+					_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT);
 				} else {
 
-					orb_publish(_actuator_output_topic, _outputs_pub, &outputs);
+					orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
 				}
 			}
 		}
@@ -747,7 +749,7 @@ PX4FMU::task_main()
 
 	}
 
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (_control_subs[i] > 0) {
 			::close(_control_subs[i]);
 			_control_subs[i] = -1;
@@ -1144,7 +1146,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		 * and PWM under control of the flight config
 		 * parameters. Note that this does not allow for
 		 * changing a set of pins to be used for serial on
-		 * FMUv1 
+		 * FMUv1
 		 */
 		switch (arg) {
 		case 0:
@@ -1637,12 +1639,15 @@ sensor_reset(int ms)
 
 	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
-	if (fd < 0)
+	if (fd < 0) {
 		errx(1, "open fail");
+	}
 
-	if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0)
-		err(1, "servo arm failed");
+	if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) {
+		warnx("sensor rail reset failed");
+	}
 
+	close(fd);
 }
 
 void

src/drivers/px4io/px4io.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -75,6 +75,10 @@
 #include <systemlib/circuit_breaker.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/safety.h>
@@ -834,7 +838,7 @@ PX4IO::init()
 	_task = task_spawn_cmd("px4io",
 					SCHED_DEFAULT,
 					SCHED_PRIORITY_ACTUATOR_OUTPUTS,
-					2000,
+					1800,
 					(main_t)&PX4IO::task_main_trampoline,
 					nullptr);
 
@@ -1102,7 +1106,7 @@ PX4IO::io_set_control_state(unsigned group)
 	uint16_t 		regs[_max_actuators];
 
 	/* get controls */
-	bool changed;
+	bool changed = false;
 
 	switch (group) {
 		case 0:
@@ -1144,11 +1148,13 @@ PX4IO::io_set_control_state(unsigned group)
 		break;
 	}
 
-	if (!changed)
+	if (!changed) {
 		return -1;
+	}
 
-	for (unsigned i = 0; i < _max_controls; i++)
+	for (unsigned i = 0; i < _max_controls; i++) {
 		regs[i] = FLOAT_TO_REG(controls.control[i]);
+	}
 
 	/* copy values to registers in IO */
 	return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
@@ -1197,7 +1203,7 @@ PX4IO::io_set_arming_state()
 
 		if (armed.ready_to_arm) {
 			set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
-			
+
 		} else {
 			clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 		}
@@ -1679,17 +1685,12 @@ PX4IO::io_publish_pwm_outputs()
 
 	/* lazily advertise on first publication */
 	if (_to_outputs == 0) {
-		_to_outputs = orb_advertise((_primary_pwm_device ?
-					     ORB_ID_VEHICLE_CONTROLS :
-					     ORB_ID(actuator_outputs_1)),
-					    &outputs);
+		int instance;
+		_to_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
+					    &outputs, &instance, ORB_PRIO_MAX);
 
 	} else {
-		orb_publish((_primary_pwm_device ?
-			     ORB_ID_VEHICLE_CONTROLS :
-			     ORB_ID(actuator_outputs_1)),
-			    _to_outputs,
-			    &outputs);
+		orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs);
 	}
 
 	return OK;
@@ -1773,12 +1774,12 @@ PX4IO::print_debug()
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 	int io_fd = -1;
 
-	if (io_fd < 0) {
+	if (io_fd <= 0) {
 		io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY);
 	}
 
 	/* read IO's output */
-	if (io_fd > 0) {
+	if (io_fd >= 0) {
 		pollfd fds[1];
 		fds[0].fd = io_fd;
 		fds[0].events = POLLIN;
@@ -2278,6 +2279,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 
 	case PWM_SERVO_GET_DISABLE_LOCKDOWN:
 		*(unsigned *)arg = _lockdown_override;
+		break;
 
 	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
 		/* force safety swith off */
@@ -2587,7 +2589,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 
 	case PWM_SERVO_SET_RC_CONFIG: {
 		/* enable setting of RC configuration without relying
-		   on param_get() 
+		   on param_get()
 		*/
 		struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
 		if (config->channel >= RC_INPUT_MAX_CHANNELS) {
@@ -2853,10 +2855,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);
 }
 
@@ -3003,11 +3005,14 @@ monitor(void)
 
 		fds[0].fd = 0;
 		fds[0].events = POLLIN;
-		poll(fds, 1, 2000);
+		if (poll(fds, 1, 2000) < 0) {
+			errx(1, "poll fail");
+		}
 
 		if (fds[0].revents == POLLIN) {
-			int c;
-			read(0, &c, 1);
+			/* control logic is to cancel with any key */
+			char c;
+			(void)read(0, &c, 1);
 
 			if (cancels-- == 0) {
 				printf("\033[2J\033[H"); /* move cursor home and clear screen */
@@ -3069,12 +3074,13 @@ lockdown(int argc, char *argv[])
 
 					if (ret > 0) {
 
-						read(0, &c, 1);
+						if (read(0, &c, 1) > 0) {
 
-						if (c != 'y') {
-							exit(0);
-						} else if (c == 'y') {
-							break;
+							if (c != 'y') {
+								exit(0);
+							} else if (c == 'y') {
+								break;
+							}
 						}
 					}
 
@@ -3237,7 +3243,13 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "limit")) {
 
 		if ((argc > 2)) {
-			g_dev->set_update_rate(atoi(argv[2]));
+			int limitrate = atoi(argv[2]);
+
+			if (limitrate > 0) {
+				g_dev->set_update_rate(limitrate);
+			} else {
+				errx(1, "invalid rate: %d", limitrate);
+			}
 
 		} else {
 			errx(1, "missing argument (50 - 500 Hz)");

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;

src/drivers/roboclaw/RoboClaw.cpp

@@ -54,6 +54,7 @@
 #include <mavlink/mavlink_log.h>
 
 #include <uORB/Publication.hpp>
+#include <uORB/topics/actuator_controls_0.h>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_hrt.h>
 
@@ -114,7 +115,7 @@ int RoboClaw::readEncoder(e_motor motor)
 	uint8_t rbuf[50];
 	usleep(5000);
 	int nread = read(_uart, rbuf, 50);
-	if (nread < 6) { 
+	if (nread < 6) {
 		printf("failed to read\n");
 		return -1;
 	}
@@ -131,7 +132,7 @@ int RoboClaw::readEncoder(e_motor motor)
 	countBytes[0] = rbuf[3];
 	uint8_t status = rbuf[4];
 	uint8_t checksum = rbuf[5];
-	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & 
+	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
 		checksum_mask;
 	// check if checksum is valid
 	if (checksum != checksum_computed) {
@@ -156,11 +157,11 @@ int RoboClaw::readEncoder(e_motor motor)
 	static int64_t overflowAmount = 0x100000000LL;
 	if (motor == MOTOR_1) {
 		_motor1Overflow += overFlow;
-		_motor1Position = float(int64_t(count) + 
+		_motor1Position = float(int64_t(count) +
 			_motor1Overflow*overflowAmount)/_pulsesPerRev;
 	} else if (motor == MOTOR_2) {
 		_motor2Overflow += overFlow;
-		_motor2Position = float(int64_t(count) + 
+		_motor2Position = float(int64_t(count) +
 			_motor2Overflow*overflowAmount)/_pulsesPerRev;
 	}
 	return 0;
@@ -242,7 +243,7 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
 	return -1;
 }
 
-int RoboClaw::resetEncoders() 
+int RoboClaw::resetEncoders()
 {
 	uint16_t sum = 0;
 	return _sendCommand(CMD_RESET_ENCODERS,
@@ -278,7 +279,7 @@ uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
 	return sum;
 }
 
-int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, 
+int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
 		size_t n_data, uint16_t & prev_sum)
 {
 	tcflush(_uart, TCIOFLUSH); // flush  buffers
@@ -299,7 +300,7 @@ int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
 	return write(_uart, buf, n_data + 3);
 }
 
-int roboclawTest(const char *deviceName, uint8_t address, 
+int roboclawTest(const char *deviceName, uint8_t address,
 		uint16_t pulsesPerRev)
 {
 	printf("roboclaw test: starting\n");

src/examples/fixedwing_control/main.c

@@ -61,6 +61,10 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/parameter_update.h>
@@ -108,7 +112,7 @@ static void usage(const char *reason);
  * @param att The current attitude. The controller should make the attitude match the setpoint
  * @param rates_sp The angular rate setpoint. This is the output of the controller.
  */
-void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, 
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
 	struct actuator_controls_s *actuators);
 
 /**
@@ -133,18 +137,18 @@ static int deamon_task;				/**< Handle of deamon task / thread */
 static struct params p;
 static struct param_handles ph;
 
-void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, 
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
 	struct actuator_controls_s *actuators)
 {
 
-	/* 
+	/*
 	 * The PX4 architecture provides a mixer outside of the controller.
 	 * The mixer is fed with a default vector of actuator controls, representing
 	 * moments applied to the vehicle frame. This vector
 	 * is structured as:
 	 *
 	 * Control Group 0 (attitude):
-	 * 
+	 *
 	 *    0  -  roll   (-1..+1)
 	 *    1  -  pitch  (-1..+1)
 	 *    2  -  yaw    (-1..+1)
@@ -226,7 +230,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	 *
 	 * Wikipedia description:
 	 * http://en.wikipedia.org/wiki/Publish–subscribe_pattern
-	 * 
+	 *
 	 */
 
 
@@ -234,7 +238,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 
 	/*
 	 * Declare and safely initialize all structs to zero.
-	 * 
+	 *
 	 * These structs contain the system state and things
 	 * like attitude, position, the current waypoint, etc.
 	 */
@@ -300,7 +304,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 		if (ret < 0) {
 			/*
 			 * Poll error, this will not really happen in practice,
-			 * but its good design practice to make output an error message. 
+			 * but its good design practice to make output an error message.
 			 */
 			warnx("poll error");
 
@@ -340,7 +344,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 				}
 
 				if (manual_sp_updated)
-					/* get the RC (or otherwise user based) input */ 
+					/* get the RC (or otherwise user based) input */
 					orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
 
 				/* check if the throttle was ever more than 50% - go later only to failsafe if yes */

src/examples/hwtest/hwtest.c

@@ -47,6 +47,10 @@
 #include <systemlib/err.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/uORB.h>
 
 __EXPORT int ex_hwtest_main(int argc, char *argv[]);

src/examples/matlab_csv_serial/matlab_csv_serial.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.		 
+ *   Copyright (c) 2014-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
@@ -190,10 +190,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
 	struct gyro_report gyro1;
 
 	/* subscribe to parameter changes */
-	int accel0_sub = orb_subscribe(ORB_ID(sensor_accel0));
-	int accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
-	int gyro0_sub = orb_subscribe(ORB_ID(sensor_gyro0));
-	int gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
+	int accel0_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
+	int accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1);
+	int gyro0_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
+	int gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1);
 
 	thread_running = true;
 
@@ -224,10 +224,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
 			/* accel0 update available? */
 			if (fds[0].revents & POLLIN)
 			{
-				orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0);
-				orb_copy(ORB_ID(sensor_accel1), accel1_sub, &accel1);
-				orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0);
-				orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1);
+				orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0);
+				orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1);
+				orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0);
+				orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1);
 
 				// write out on accel 0, but collect for all other sensors as they have updates
 				dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw,

src/examples/publisher/module.mk

@@ -38,6 +38,7 @@
 MODULE_COMMAND	= publisher
 
 SRCS		= publisher_main.cpp \
+		  publisher_start_nuttx.cpp \
 		  publisher_example.cpp
 
 MODULE_STACKSIZE = 1200

src/examples/publisher/publisher_example.cpp

@@ -45,9 +45,10 @@ using namespace px4;
 
 PublisherExample::PublisherExample() :
 	_n(),
-	_rc_channels_pub(PX4_ADVERTISE(_n, rc_channels))
+	_rc_channels_pub(_n.advertise<px4_rc_channels>()),
+	_v_att_pub(_n.advertise<px4_vehicle_attitude>()),
+	_parameter_update_pub(_n.advertise<px4_parameter_update>())
 {
-
 }
 
 int PublisherExample::main()
@@ -55,14 +56,27 @@ int PublisherExample::main()
 	px4::Rate loop_rate(10);
 
 	while (px4::ok()) {
-		PX4_TOPIC_T(rc_channels) msg;
-		msg.timestamp_last_valid = px4::get_time_micros();
-		PX4_INFO("%llu", msg.timestamp_last_valid);
-
-		_rc_channels_pub->publish(msg);
-
-		_n.spinOnce();
 		loop_rate.sleep();
+		_n.spinOnce();
+
+		/* Publish example message */
+		px4_rc_channels rc_channels_msg;
+		rc_channels_msg.data().timestamp_last_valid = px4::get_time_micros();
+		PX4_INFO("rc: %llu", rc_channels_msg.data().timestamp_last_valid);
+		_rc_channels_pub->publish(rc_channels_msg);
+
+		/* Publish example message */
+		px4_vehicle_attitude v_att_msg;
+		v_att_msg.data().timestamp = px4::get_time_micros();
+		PX4_INFO("att: %llu", v_att_msg.data().timestamp);
+		_v_att_pub->publish(v_att_msg);
+
+		/* Publish example message */
+		px4_parameter_update parameter_update_msg;
+		parameter_update_msg.data().timestamp = px4::get_time_micros();
+		PX4_INFO("param update: %llu", parameter_update_msg.data().timestamp);
+		_parameter_update_pub->publish(parameter_update_msg);
+
 	}
 
 	return 0;

src/examples/publisher/publisher_example.h

@@ -37,6 +37,7 @@
  *
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
+#pragma once
 #include <px4.h>
 
 class PublisherExample {
@@ -48,5 +49,7 @@ public:
 	int main();
 protected:
 	px4::NodeHandle _n;
-	px4::Publisher * _rc_channels_pub;
+	px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub;
+	px4::Publisher<px4::px4_vehicle_attitude> * _v_att_pub;
+	px4::Publisher<px4::px4_parameter_update> * _parameter_update_pub;
 };

src/examples/publisher/publisher_main.cpp

@@ -37,70 +37,11 @@
  *
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
-#include <string.h>
-#include <cstdlib>
 #include "publisher_example.h"
 
-static bool thread_running = false;     /**< Deamon status flag */
-static int daemon_task;             /**< Handle of deamon task / thread */
-namespace px4
-{
-bool task_should_exit = false;
-}
-using namespace px4;
+bool thread_running = false;     /**< Deamon status flag */
 
-PX4_MAIN_FUNCTION(publisher);
-
-#if !defined(__PX4_ROS)
-extern "C" __EXPORT int publisher_main(int argc, char *argv[]);
-int publisher_main(int argc, char *argv[])
-{
-	if (argc < 1) {
-		errx(1, "usage: publisher {start|stop|status}");
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("already running");
-			/* this is not an error */
-			exit(0);
-		}
-
-		task_should_exit = false;
-
-		daemon_task = task_spawn_cmd("publisher",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2000,
-				       publisher_task_main,
-					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
-
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		task_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("is running");
-
-		} else {
-			warnx("not started");
-		}
-
-		exit(0);
-	}
-
-	warnx("unrecognized command");
-	return 1;
-}
-#endif
-
-PX4_MAIN_FUNCTION(publisher)
+int main(int argc, char **argv)
 {
 	px4::init(argc, argv, "publisher");
 

src/examples/publisher/publisher_start_nuttx.cpp

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   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 publisher_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int publisher_main(int argc, char *argv[]);
+int publisher_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: publisher {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("publisher",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       main,
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}

src/examples/subscriber/module.mk

@@ -38,6 +38,7 @@
 MODULE_COMMAND	= subscriber
 
 SRCS		= subscriber_main.cpp \
+		  subscriber_start_nuttx.cpp \
 		  subscriber_example.cpp \
 		  subscriber_params.c
 

src/examples/subscriber/subscriber_example.cpp

@@ -43,8 +43,8 @@
 
 using namespace px4;
 
-void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid);
+void rc_channels_callback_function(const px4_rc_channels &msg) {
+	PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid);
 }
 
 SubscriberExample::SubscriberExample() :
@@ -62,11 +62,19 @@ SubscriberExample::SubscriberExample() :
 
 	/* Do some subscriptions */
 	/* Function */
-	PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
-	/* Class Method */
-	PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
+	_n.subscribe<px4_rc_channels>(rc_channels_callback_function, _interval);
+
 	/* No callback */
-	_sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500);
+	_sub_rc_chan = _n.subscribe<px4_rc_channels>(500);
+
+	/* Class method */
+	_n.subscribe<px4_rc_channels>(&SubscriberExample::rc_channels_callback, this, 1000);
+
+	/* Another class method */
+	_n.subscribe<px4_vehicle_attitude>(&SubscriberExample::vehicle_attitude_callback, this, 1000);
+
+	/* Yet antoher class method */
+	_n.subscribe<px4_parameter_update>(&SubscriberExample::parameter_update_callback, this, 1000);
 
 	PX4_INFO("subscribed");
 }
@@ -75,8 +83,23 @@ SubscriberExample::SubscriberExample() :
  * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
  * Also the current value of the _sub_rc_chan subscription is printed
  */
-void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
-			msg.timestamp_last_valid,
-			_sub_rc_chan->get().timestamp_last_valid);
+void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) {
+	PX4_INFO("rc_channels_callback (method): [%llu]",
+			msg.data().timestamp_last_valid);
+	PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]",
+			_sub_rc_chan->data().timestamp_last_valid);
+}
+
+void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) {
+	PX4_INFO("vehicle_attitude_callback (method): [%llu]",
+			msg.data().timestamp);
+}
+
+void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) {
+	PX4_INFO("parameter_update_callback (method): [%llu]",
+			msg.data().timestamp);
+	PX4_PARAM_GET(_p_sub_interv, &_interval);
+	PX4_INFO("Param SUB_INTERV = %d", _interval);
+	PX4_PARAM_GET(_p_test_float, &_test_float);
+	PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float);
 }

src/examples/subscriber/subscriber_example.h

@@ -41,7 +41,7 @@
 
 using namespace px4;
 
-void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg);
+void rc_channels_callback_function(const px4_rc_channels &msg);
 
 class SubscriberExample {
 public:
@@ -56,9 +56,10 @@ protected:
 	int32_t _interval;
 	px4_param_t _p_test_float;
 	float _test_float;
-	px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan;
-
-	void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);
+	px4::Subscriber<px4_rc_channels> * _sub_rc_chan;
 
+	void rc_channels_callback(const px4_rc_channels &msg);
+	void vehicle_attitude_callback(const px4_vehicle_attitude &msg);
+	void parameter_update_callback(const px4_parameter_update &msg);
 
 };

src/examples/subscriber/subscriber_main.cpp

@@ -37,70 +37,10 @@
  *
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
-#include <string.h>
-#include <cstdlib>
 #include "subscriber_example.h"
+bool thread_running = false;     /**< Deamon status flag */
 
-static bool thread_running = false;     /**< Deamon status flag */
-static int daemon_task;             /**< Handle of deamon task / thread */
-namespace px4
-{
-bool task_should_exit = false;
-}
-using namespace px4;
-
-PX4_MAIN_FUNCTION(subscriber);
-
-#if !defined(__PX4_ROS)
-extern "C" __EXPORT int subscriber_main(int argc, char *argv[]);
-int subscriber_main(int argc, char *argv[])
-{
-	if (argc < 1) {
-		errx(1, "usage: subscriber {start|stop|status}");
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("already running");
-			/* this is not an error */
-			exit(0);
-		}
-
-		task_should_exit = false;
-
-		daemon_task = task_spawn_cmd("subscriber",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2000,
-				       subscriber_task_main,
-					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
-
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		task_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("is running");
-
-		} else {
-			warnx("not started");
-		}
-
-		exit(0);
-	}
-
-	warnx("unrecognized command");
-	return 1;
-}
-#endif
-
-PX4_MAIN_FUNCTION(subscriber)
+int main(int argc, char **argv)
 {
 	px4::init(argc, argv, "subscriber");
 

src/examples/subscriber/subscriber_start_nuttx.cpp

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   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 subscriber_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int subscriber_main(int argc, char *argv[]);
+int subscriber_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: subscriber {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("subscriber",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       main,
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}

src/lib/mathlib/math/filter/LowPassFilter2p.cpp

@@ -66,6 +66,7 @@ float LowPassFilter2p::apply(float sample)
         // no filtering
         return sample;
     }
+
     // do the filtering
     float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
     if (isnan(delay_element_0) || isinf(delay_element_0)) {
@@ -82,7 +83,9 @@ float LowPassFilter2p::apply(float sample)
 }
 
 float LowPassFilter2p::reset(float sample) {
-    _delay_element_1 = _delay_element_2 = sample;
+	float dval = sample / (_b0 + _b1 + _b2);
+    _delay_element_1 = dval;
+    _delay_element_2 = dval;
     return apply(sample);
 }
 

src/lib/uavcan

@@ -1 +1 @@
-Subproject commit c4c45b995f5c8192c7a36c4293c201711ceac74d
+Subproject commit 7719f7692aba67f01b6321773bb7be13f23d2f68

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -595,44 +595,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
 					att.R_valid = true;
 
-					// compute secondary attitude
-					math::Matrix<3, 3> R_adapted;		//modified rotation matrix
-					R_adapted = R;
-
-					//move z to x
-					R_adapted(0, 0) = R(0, 2);
-					R_adapted(1, 0) = R(1, 2);
-					R_adapted(2, 0) = R(2, 2);
-					//move x to z
-					R_adapted(0, 2) = R(0, 0);
-					R_adapted(1, 2) = R(1, 0);
-					R_adapted(2, 2) = R(2, 0);
-
-					//change direction of pitch (convert to right handed system)
-					R_adapted(0, 0) = -R_adapted(0, 0);
-					R_adapted(1, 0) = -R_adapted(1, 0);
-					R_adapted(2, 0) = -R_adapted(2, 0);
-					math::Vector<3> euler_angles_sec;		//adapted euler angles for fixed wing operation
-					euler_angles_sec = R_adapted.to_euler();
-
-					att.roll_sec    = euler_angles_sec(0);
-					att.pitch_sec   = euler_angles_sec(1);
-					att.yaw_sec     = euler_angles_sec(2);
-
-					memcpy(&att.R_sec[0], &R_adapted.data[0][0], sizeof(att.R_sec));
-
-					att.rollspeed_sec  = -x_aposteriori[2];
-					att.pitchspeed_sec = x_aposteriori[1];
-					att.yawspeed_sec   = x_aposteriori[0];
-					att.rollacc_sec    = -x_aposteriori[5];
-					att.pitchacc_sec   = x_aposteriori[4];
-					att.yawacc_sec     = x_aposteriori[3];
-
-					att.g_comp_sec[0] = -raw.accelerometer_m_s2[2] - (-acc(2));
-					att.g_comp_sec[1] = raw.accelerometer_m_s2[1] - acc(1);
-					att.g_comp_sec[2] = raw.accelerometer_m_s2[0] - acc(0);
-
-
 					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
 						// Broadcast
 						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);

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

src/modules/bottle_drop/bottle_drop.cpp

@@ -57,6 +57,10 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/wind_estimate.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_global_position.h>

src/modules/commander/accelerometer_calibration.cpp

@@ -159,6 +159,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 int do_accel_calibration(int mavlink_fd)
 {
 	int fd;
+	int32_t device_id;
 
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 
@@ -180,6 +181,9 @@ int do_accel_calibration(int mavlink_fd)
 
 	/* reset all offsets to zero and all scales to one */
 	fd = open(ACCEL_DEVICE_PATH, 0);
+
+	device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
+
 	res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
 	close(fd);
 
@@ -226,6 +230,10 @@ int do_accel_calibration(int mavlink_fd)
 			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			res = ERROR;
 		}
+
+		if (param_set(param_find("SENS_ACC_ID"), &(device_id))) {
+				res = ERROR;
+		}
 	}
 
 	if (res == OK) {

src/modules/commander/commander_helper.cpp

@@ -72,16 +72,16 @@ static const int ERROR = -1;
 
 bool is_multirotor(const struct vehicle_status_s *current_status)
 {
-	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
-		(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
-		(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
-		(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
+	return ((current_status->system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR) ||
+		(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR) ||
+		(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR) ||
+		(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER));
 }
 
 bool is_rotary_wing(const struct vehicle_status_s *current_status)
 {
-	return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
-	       || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
+	return is_multirotor(current_status) || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER)
+	       || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL);
 }
 
 static int buzzer = -1;

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -73,7 +73,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         bool            armed;          // actuator_armed_s.armed
         bool            ready_to_arm;   // actuator_armed_s.ready_to_arm
     } ArmingTransitionVolatileState_t;
-    
+
     // This structure represents a test case for arming_state_transition. It contains the machine
     // state prior to transtion, the requested state to transition to and finally the expected
     // machine state after transition.
@@ -88,7 +88,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         ArmingTransitionVolatileState_t expected_state;                         // Expected machine state after transition
         transition_result_t             expected_transition_result;             // Expected result from arming_state_transition
     } ArmingTransitionTest_t;
-    
+
     // We use these defines so that our test cases are more readable
     #define ATT_ARMED true
     #define ATT_DISARMED false
@@ -100,182 +100,182 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
     #define ATT_SAFETY_NOT_AVAILABLE true
     #define ATT_SAFETY_OFF true
     #define ATT_SAFETY_ON false
-    
+
     // These are test cases for arming_state_transition
     static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
         // TRANSITION_NOT_CHANGED tests
-        
+
         { "no transition: identical states",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_INIT,
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_INIT,
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
+
         // TRANSITION_CHANGED tests
-        
+
         // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
-        
+
         { "transition: init to standby",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: init to standby error",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY_ERROR,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: init to reboot",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby to init",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_INIT,
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_INIT,
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby to standby error",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY_ERROR,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby to reboot",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: armed to standby",
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: armed to armed error",
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED_ERROR,
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED_ERROR,
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: armed error to standby error",
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY_ERROR,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby error to reboot",
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: in air restore to armed",
-            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: in air restore to reboot",
-            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         // hil on tests, standby error to standby not normally allowed
-        
+
         { "transition: standby error to standby, hil on",
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         // Safety switch arming tests
-        
+
         { "transition: standby to armed, no safety switch",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby to armed, safety switch off",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         // standby error
         { "transition: armed error to standby error requested standby",
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         // TRANSITION_DENIED tests
-        
+
         // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
-        
+
         { "no transition: init to armed",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: standby to armed error",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED_ERROR,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED_ERROR,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: armed to init",
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_INIT,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_INIT,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: armed to reboot",
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: armed error to armed",
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: armed error to reboot",
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: standby error to armed",
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: standby error to standby",
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: reboot to armed",
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: in air restore to standby",
-            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         // Sensor tests
-        
+
         { "no transition: init to standby - sensors not initialized",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         // Safety switch arming tests
-        
+
         { "no transition: init to standby, safety switch on",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
     };
-    
+
 	struct vehicle_status_s status;
 	struct safety_s         safety;
 	struct actuator_armed_s armed;
-    
+
     size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
     for (size_t i=0; i<cArmingTransitionTests; i++) {
         const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
-        
+
         // Setup initial machine state
         status.arming_state = test->current_state.arming_state;
         status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
@@ -286,10 +286,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         safety.safety_off = test->safety_off;
         armed.armed = test->current_state.armed;
         armed.ready_to_arm = test->current_state.ready_to_arm;
-        
+
         // Attempt transition
         transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
-        
+
         // Validate result of transition
         ut_assert(test->assertMsg, test->expected_transition_result == result);
         ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state);
@@ -310,7 +310,7 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 		main_state_t	to_state;				// State to transition to
 		transition_result_t	expected_transition_result;	// Expected result from main_state_transition call
 	} MainTransitionTest_t;
-	
+
 	// Bits for condition_bits
 	#define MTT_ALL_NOT_VALID		0
 	#define MTT_ROTARY_WING			1 << 0
@@ -318,108 +318,108 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 	#define MTT_LOC_POS_VALID		1 << 2
 	#define MTT_HOME_POS_VALID		1 << 3
 	#define MTT_GLOBAL_POS_VALID		1 << 4
-	
+
 	static const MainTransitionTest_t rgMainTransitionTests[] = {
-		
+
 		// TRANSITION_NOT_CHANGED tests
-		
+
 		{ "no transition: identical states",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
-				
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
+
 		// TRANSITION_CHANGED tests
-		
+
 		{ "transition: MANUAL to ACRO",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },
 
 		{ "transition: ACRO to MANUAL",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_ACRO, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
 
 		{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_AUTO_MISSION, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_LOITER - global position valid",
 			MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
 
 		{ "transition: AUTO_LOITER to MANUAL - global position valid",
 			MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_AUTO_LOITER, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
 
 		{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_AUTO_RTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to ALTCTL - not rotary",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
 			MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+
 		{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
 			MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+
 		{ "transition: ALTCTL to MANUAL - local altitude valid",
 			MTT_LOC_ALT_VALID,
-			MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_ALTCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
 			MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
 			MTT_LOC_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
 
 		{ "transition: POSCTL to MANUAL - local position valid, global position valid",
 			MTT_LOC_POS_VALID,
-			MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_POSCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		// TRANSITION_DENIED tests
 
 		{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
 			MTT_HOME_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
 
 		{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
 			MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
 			MTT_ROTARY_WING,
-			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_DENIED },
 	};
-	
+
 	size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
 	for (size_t i=0; i<cMainTransitionTests; i++) {
 		const MainTransitionTest_t* test = &rgMainTransitionTests[i];
@@ -432,10 +432,10 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 		current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
 		current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
 		current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
-		
+
 		// Attempt transition
 		transition_result_t result = main_state_transition(&current_state, test->to_state);
-		
+
 		// Validate result of transition
 		ut_assert(test->assertMsg, test->expected_transition_result == result);
 		if (test->expected_transition_result == result) {
@@ -495,8 +495,8 @@ bool StateMachineHelperTest::run_tests(void)
 	ut_run_test(armingStateTransitionTest);
 	ut_run_test(mainStateTransitionTest);
 	ut_run_test(isSafeTest);
-	
+
 	return (_tests_failed == 0);
 }
 
-ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
+ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)