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:
-#after_script:
+  - git clone git://github.com/PX4/CI-Tools.git
-#  - echo "Branch $TRAVIS_BRANCH (pull request: $TRAVIS_PULL_REQUEST) ready for flight testing." >> $PX4_REPORT
+  - ./CI-Tools/s3cmd-configure
-#  - git log -n1 > $PX4_REPORT
+# upload newest build for this branch with s3 index
-#  - echo " " >> $PX4_REPORT
+  - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
-#  - echo "Files available at:" >> $PX4_REPORT
+# archive newest build by date with s3 index
-#  - echo "https://px4-travis.s3.amazonaws.com/PX4/Firmware/$TRAVIS_BUILD_NUMBER/$TRAVIS_BUILD_NUMBER.1/Firmware.zip" >> $PX4_REPORT
+  - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/
-#  - echo "Description of desired tests is available at:" >> $PX4_REPORT
+  - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/
-#  - echo "https://github.com/PX4/Firmware/pull/$TRAVIS_PULL_REQUEST" >> $PX4_REPORT
+# upload top level index.html and timestamp.html
-#  - echo " " >> $PX4_REPORT
+  - ./CI-Tools/s3cmd-put CI-Tools/index.html index.html
-#  - echo "Thanks for testing!" >> $PX4_REPORT
+  - ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html
-#  - 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/
 
 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(parse_int(t['stack_alloc_ptr']))
-			p.append(int(t['adj_stack_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':
-		if words[0] == 'No':
-			#no line info...
-			pass
-		else:
-			valid = True
 			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
-			
+			return words[3].strip('"'), line, func
-		if valid:
-			print("Line: ",line," in ",path,"/",name,"in ",func)
-			return name,path,line,func
-			
-			
-			
 		
 	def invoke(self,args,sth):
-		addr_dec = int(args[2:],16)
+		try:
-		_tcb = self.get_tcb_from_address(addr_dec)
+			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'])
+		up_stack = parse_int(_tcb['adj_stack_ptr'])
-		curr_sp = int(_tcb['xcp']['regs'][0]) #curr stack pointer
+		curr_sp = parse_int(_tcb['xcp']['regs'][0]) #curr stack pointer
-		other_sp = int(_tcb['xcp']['regs'][8]) # other stack pointer
+		other_sp = parse_int(_tcb['xcp']['regs'][8]) # other stack pointer
-		stacksize = int(_tcb['adj_stack_size']) # 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:
+		item = 0
-			sp = other_sp
+		for sp in range(other_sp if curr_sp == up_stack else curr_sp, up_stack, 4):
-		else: 
-			sp = curr_sp;
-			
-		while(sp < up_stack):
 			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)
+				filename,line,func = self.resolve_file_line_func(mem, stack_percentage)
-			sp = sp + 4; # jump up one word
+				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
+MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages
-GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src
+MULTIPLATFORM_PREFIX = px4_
-GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src
+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

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)
+                            self.__drawProgressBar(label, 10.0, 10.0)
-                            sys.stdout.write("\nerase complete!\n")
                             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(label, uploadProgress, len(groups))
-                self.__drawProgressBar(100, 100)
+                self.__drawProgressBar(label, 100, 100)
-                print("\nprogram complete!")
 
         # 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):
+        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()
                 
-                print("program...")
+                self.__erase("Erase  ")
-                self.__program(fw)
+                self.__program("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 \
+# Enabling stack checks if OS was build with them
-			   -ffixed-r10
+#
-
+TEST_FILE_STACKCHECK=$(WORK_DIR)nuttx-export/include/nuttx/config.h
-ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \
+TEST_VALUE_STACKCHECK=CONFIG_ARMV7M_STACKCHECK\ 1
-			   -ffixed-r10
+ENABLE_STACK_CHECKS=$(shell $(GREP) -q "$(TEST_VALUE_STACKCHECK)" $(TEST_FILE_STACKCHECK); echo $$?;)
-
+ifeq ("$(ENABLE_STACK_CHECKS)","0")
-ARCHINSTRUMENTATIONDEFINES_CORTEXM3 =
+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
+uorb_struct = '%s_s'%spec.short_name
-cpp_class = '%s_'%spec.short_name
+topic_name = 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)
 }@
 
 #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 \
+ifeq ($(CONFIG_ARMV7M_STACKCHECK),y)
-			   -ffixed-r10
+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 \
+ifeq ($(CONFIG_ARMV7M_STACKCHECK),y)
-			   -ffixed-r10
+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 \
+ifeq ($(CONFIG_ARMV7M_STACKCHECK),y)
-			   -ffixed-r10
+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>
 

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;