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

Conflicts:
	makefiles/config_px4fmu-v2_test.mk

Debug/NuttX

@@ -78,7 +78,7 @@ end
 ################################################################################
 
 define showfiles
-	set $task = (struct _TCB *)$arg0
+	set $task = (struct tcb_s *)$arg0
 	set $nfiles = sizeof((*(struct filelist*)0).fl_files) / sizeof(struct file)
 	printf "%d files\n", $nfiles
 	set $index = 0
@@ -102,7 +102,7 @@ end
 ################################################################################
 
 define _showtask_oneline
-	set $task = (struct _TCB *)$arg0
+	set $task = (struct tcb_s *)$arg0
 	printf "    %p  %.2d %.3d %s\n", $task, $task->pid, $task->sched_priority, $task->name
 end
 
@@ -139,7 +139,7 @@ end
 # Print task registers for a NuttX v7em target with FPU enabled.
 #
 define _showtaskregs_v7em
-	set $task = (struct _TCB *)$arg0
+	set $task = (struct tcb_s *)$arg0
 	set $regs = (uint32_t *)&($task->xcp.regs[0])
 
 	printf "    r0: 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x\n", $regs[27], $regs[28], $regs[29], $regs[30], $regs[2], $regs[3], $regs[4], $regs[5]
@@ -162,7 +162,7 @@ end
 define _showsemaphore
 	printf "count %d ", $arg0->semcount
 	if $arg0->holder.htcb != 0
-		set $_task = (struct _TCB *)$arg0->holder.htcb
+		set $_task = (struct tcb_s *)$arg0->holder.htcb
 		printf "held by %s", $_task->name
 	end
 	printf "\n"
@@ -172,7 +172,7 @@ end
 # Print information about a task's stack usage
 #
 define showtaskstack
-	set $task = (struct _TCB *)$arg0
+	set $task = (struct tcb_s *)$arg0
 
 	if $task == &g_idletcb
 		printf "can't measure idle stack\n"
@@ -189,7 +189,7 @@ end
 # Print details of a task
 #
 define showtask
-	set $task = (struct _TCB *)$arg0
+	set $task = (struct tcb_s *)$arg0
 
 	printf "%p %.2d ", $task, $task->pid
 	_showtaskstate $task
@@ -204,7 +204,7 @@ define showtask
 	if $task->task_state != TSTATE_TASK_RUNNING
 		_showtaskregs_v7em $task
 	else
-		_showcurrentregs_v7em
+		_showtaskregs_v7em $task
 	end
 
 	# XXX print registers here
@@ -247,8 +247,10 @@ define showtasks
 	_showtasklist &g_pendingtasks
 	printf "RUNNABLE\n"
 	_showtasklist &g_readytorun
-	printf "WAITING\n"
+	printf "WAITING for Semaphore\n"
 	_showtasklist &g_waitingforsemaphore
+	printf "WAITING for Signal\n"
+	_showtasklist &g_waitingforsignal
 	printf "INACTIVE\n"
 	_showtasklist &g_inactivetasks
 end
@@ -257,3 +259,23 @@ document showtasks
 .    showtasks
 .        Print a list of all tasks in the system, separated into their respective queues.
 end
+
+define my_mem
+
+    set $start = $arg0
+    set $end = $arg1
+    set $cursor = $start
+    
+    if $start < $end
+      while $cursor != $end
+        set *$cursor = 0x0000
+        set $cursor = $cursor + 4
+        printf "0x%x of 0x%x\n",$cursor,$end 
+      end
+    else
+      while $cursor != $end
+        set *$cursor = 0x0000
+        set $cursor = $cursor - 4
+      end
+    end
+end

Debug/Nuttx.py

@@ -59,30 +59,42 @@ class NX_register_set(object):
 
 	def __init__(self, xcpt_regs):
 		if xcpt_regs is None:
-			self.regs['R0']         = long(gdb.parse_and_eval('$r0'))
-			self.regs['R1']         = long(gdb.parse_and_eval('$r1'))
-			self.regs['R2']         = long(gdb.parse_and_eval('$r2'))
-			self.regs['R3']         = long(gdb.parse_and_eval('$r3'))
-			self.regs['R4']         = long(gdb.parse_and_eval('$r4'))
-			self.regs['R5']         = long(gdb.parse_and_eval('$r5'))
-			self.regs['R6']         = long(gdb.parse_and_eval('$r6'))
-			self.regs['R7']         = long(gdb.parse_and_eval('$r7'))
-			self.regs['R8']         = long(gdb.parse_and_eval('$r8'))
-			self.regs['R9']         = long(gdb.parse_and_eval('$r9'))
-			self.regs['R10']        = long(gdb.parse_and_eval('$r10'))
-			self.regs['R11']        = long(gdb.parse_and_eval('$r11'))
-			self.regs['R12']        = long(gdb.parse_and_eval('$r12'))
-			self.regs['R13']        = long(gdb.parse_and_eval('$r13'))
-			self.regs['SP']         = long(gdb.parse_and_eval('$sp'))
-			self.regs['R14']        = long(gdb.parse_and_eval('$r14'))
-			self.regs['LR']         = long(gdb.parse_and_eval('$lr'))
-			self.regs['R15']        = long(gdb.parse_and_eval('$r15'))
-			self.regs['PC']         = long(gdb.parse_and_eval('$pc'))
-			self.regs['XPSR']       = long(gdb.parse_and_eval('$xpsr'))
+			self.regs['R0']         = self.mon_reg_call('r0')
+			self.regs['R1']         = self.mon_reg_call('r1')
+			self.regs['R2']         = self.mon_reg_call('r2')
+			self.regs['R3']         = self.mon_reg_call('r3')
+			self.regs['R4']         = self.mon_reg_call('r4')
+			self.regs['R5']         = self.mon_reg_call('r5')
+			self.regs['R6']         = self.mon_reg_call('r6')
+			self.regs['R7']         = self.mon_reg_call('r7')
+			self.regs['R8']         = self.mon_reg_call('r8')
+			self.regs['R9']         = self.mon_reg_call('r9')
+			self.regs['R10']        = self.mon_reg_call('r10')
+			self.regs['R11']        = self.mon_reg_call('r11')
+			self.regs['R12']        = self.mon_reg_call('r12')
+			self.regs['R13']        = self.mon_reg_call('r13')
+			self.regs['SP']         = self.mon_reg_call('sp')
+			self.regs['R14']        = self.mon_reg_call('r14')
+			self.regs['LR']         = self.mon_reg_call('lr')
+			self.regs['R15']        = self.mon_reg_call('r15')
+			self.regs['PC']         = self.mon_reg_call('pc')
+			self.regs['XPSR']       = self.mon_reg_call('xPSR')
 		else:
 			for key in self.v7em_regmap.keys():
 				self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]])
 
+	def mon_reg_call(self,register):
+		"""
+		register is the register as a string e.g. 'pc'
+		return integer containing the value of the register
+		"""
+		str_to_eval = "mon reg "+register
+		resp = gdb.execute(str_to_eval,to_string = True)
+		content = resp.split()[-1];
+		try:
+			return int(content,16)
+		except:
+			return 0
 
 	@classmethod
 	def with_xcpt_regs(cls, xcpt_regs):
@@ -172,7 +184,7 @@ class NX_task(object):
 				self.__dict__['stack_used'] = 0
 			else:
 				stack_limit = self._tcb['adj_stack_size']
-				for offset in range(0, stack_limit):
+				for offset in range(0, int(stack_limit)):
 					if stack_base[offset] != 0xff:
 						break
 				self.__dict__['stack_used'] = stack_limit - offset
@@ -187,7 +199,7 @@ class NX_task(object):
 	def state(self):
 		"""return the name of the task's current state"""
 		statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e'))
-		for name,value in statenames.iteritems():
+		for name,value in statenames.items():
 			if value == self._tcb['task_state']:
 				return name
 		return 'UNKNOWN'
@@ -196,16 +208,19 @@ class NX_task(object):
 	def waiting_for(self):
 		"""return a description of what the task is waiting for, if it is waiting"""
 		if self._state_is('TSTATE_WAIT_SEM'):
-			waitsem = self._tcb['waitsem'].dereference()
-			waitsem_holder = waitsem['holder']
-			holder = NX_task.for_tcb(waitsem_holder['htcb'])
-			if holder is not None:
-				return '{}({})'.format(waitsem.address, holder.name)
-			else:
-				return '{}(<bad holder>)'.format(waitsem.address)
+			try: 
+				waitsem = self._tcb['waitsem'].dereference()
+				waitsem_holder = waitsem['holder']
+				holder = NX_task.for_tcb(waitsem_holder['htcb'])
+				if holder is not None:
+					return '{}({})'.format(waitsem.address, holder.name)
+				else:
+					return '{}(<bad holder>)'.format(waitsem.address)
+			except:
+				return 'EXCEPTION'
 		if self._state_is('TSTATE_WAIT_SIG'):
 			return 'signal'
-		return None
+		return ""
 
 	@property
 	def is_waiting(self):
@@ -229,7 +244,7 @@ class NX_task(object):
 		filearray = filelist['fl_files']
 		result = dict()
 		for i in range(filearray.type.range()[0],filearray.type.range()[1]):
-			inode = long(filearray[i]['f_inode'])
+			inode = int(filearray[i]['f_inode'])
 			if inode != 0:
 				result[i] = inode
 		return result
@@ -253,7 +268,18 @@ class NX_task(object):
 
 	def __str__(self):
 		return "{}:{}".format(self.pid, self.name)
-
+	
+	def showoff(self):
+		print("-------")
+		print(self.pid,end = ", ")
+		print(self.name,end = ", ")
+		print(self.state,end = ", ")
+		print(self.waiting_for,end = ", ")
+		print(self.stack_used,end = ", ")
+		print(self._tcb['adj_stack_size'],end = ", ")
+		print(self.file_descriptors)
+		print(self.registers)
+		
 	def __format__(self, format_spec):
 		return format_spec.format(
 			pid              = self.pid,
@@ -265,7 +291,7 @@ class NX_task(object):
 			file_descriptors = self.file_descriptors,
 			registers	 = self.registers
 			)
-
+	
 class NX_show_task (gdb.Command):
 	"""(NuttX) prints information about a task"""
 
@@ -285,7 +311,7 @@ class NX_show_task (gdb.Command):
 			my_fmt += '  R8  {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n'
 			my_fmt += '  R12 {registers[PC]:#010x}\n'
 			my_fmt += '  SP  {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n'
-			print format(t, my_fmt)
+			print(format(t, my_fmt))
 
 class NX_show_tasks (gdb.Command):
 	"""(NuttX) prints a list of tasks"""
@@ -295,8 +321,10 @@ class NX_show_tasks (gdb.Command):
 
 	def invoke(self, args, from_tty):
 		tasks = NX_task.tasks()
+		print ('Number of tasks: ' + str(len(tasks)))
 		for t in tasks:
-			print format(t, '{pid:<2} {name:<16} {state:<20} {stack_used:>4}/{stack_limit:<4}')
+			#t.showoff()
+			print(format(t, 'Task: {pid} {name} {state} {stack_used}/{stack_limit}'))
 
 NX_show_task()
 NX_show_tasks()
@@ -306,15 +334,15 @@ class NX_show_heap (gdb.Command):
 
 	def __init__(self):
 		super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER)
-                struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s')
-                preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof
-                if preceding_size == 2:
+		struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s')
+		preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof
+		if preceding_size == 2:
 			self._allocflag = 0x8000
-                elif preceding_size == 4:
+		elif preceding_size == 4:
 			self._allocflag = 0x80000000
-                else:
-                        raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size)
-                self._allocnodesize = struct_mm_allocnode_s.sizeof
+		else:
+			raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size)
+			self._allocnodesize = struct_mm_allocnode_s.sizeof
 
 	def _node_allocated(self, allocnode):
 		if allocnode['preceding'] & self._allocflag:
@@ -328,7 +356,7 @@ class NX_show_heap (gdb.Command):
 		if region_start >= region_end:
 			raise gdb.GdbError('heap region {} corrupt'.format(hex(region_start)))
 		nodecount = region_end - region_start
-		print 'heap {} - {}'.format(region_start, region_end)
+		print ('heap {} - {}'.format(region_start, region_end))
 		cursor = 1
 		while cursor < nodecount:
 			allocnode = region_start[cursor]
@@ -336,8 +364,8 @@ class NX_show_heap (gdb.Command):
 				state = ''
 			else:
 				state = '(free)'
-			print '  {} {} {}'.format(allocnode.address + self._allocnodesize,
-                                                  self._node_size(allocnode), state)
+			print( '  {} {} {}'.format(allocnode.address + self._allocnodesize,
+                                                  self._node_size(allocnode), state))
 			cursor += self._node_size(allocnode) / self._allocnodesize
 
 	def invoke(self, args, from_tty):
@@ -345,7 +373,7 @@ class NX_show_heap (gdb.Command):
 		nregions = heap['mm_nregions']
 		region_starts = heap['mm_heapstart']
 		region_ends = heap['mm_heapend']
-		print '{} heap(s)'.format(nregions)
+		print( '{} heap(s)'.format(nregions))
 		# walk the heaps
 		for i in range(0, nregions):
 			self._print_allocations(region_starts[i], region_ends[i])
@@ -370,6 +398,317 @@ class NX_show_interrupted_thread (gdb.Command):
 			my_fmt += '  R8  {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n'
 			my_fmt += '  R12 {registers[PC]:#010x}\n'
 			my_fmt += '  SP  {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n'
-			print format(registers, my_fmt)
+			print (format(registers, my_fmt))
 
 NX_show_interrupted_thread()
+
+class NX_check_tcb(gdb.Command):
+	""" check the tcb of a task from a address """
+	def __init__(self):
+		super(NX_check_tcb,self).__init__('show tcb', gdb.COMMAND_USER)
+		
+	def invoke(self,args,sth):
+		tasks = NX_task.tasks()
+		print("tcb int: ",int(args))
+		print(tasks[int(args)]._tcb)
+		a =tasks[int(args)]._tcb['xcp']['regs']
+		print("relevant registers:")
+		for reg in regmap:
+			hex_addr= hex(int(a[regmap[reg]]))
+			eval_string = 'info line *'+str(hex_addr)
+			print(reg,": ",hex_addr,)
+NX_check_tcb()
+
+class NX_tcb(object):
+	def __init__(self):
+		pass
+	
+	def is_in(self,arg,list):
+		for i in list:
+			if arg == i:
+				return True;
+		return False
+	
+	def find_tcb_list(self,dq_entry_t):
+		tcb_list = []
+		tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer())
+		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]):
+			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]
+	
+	def getTCB(self):
+		list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks']
+		tcb_list = [];
+		for l in list_of_listsnames:
+			li = gdb.lookup_global_symbol(l)
+			print(li)
+			cursor = li.value()['head']
+			tcb_list = tcb_list + self.find_tcb_list(cursor)
+
+class NX_check_stack_order(gdb.Command):
+	""" Check the Stack order corresponding to the tasks """
+	
+	def __init__(self):
+		super(NX_check_stack_order,self).__init__('show check_stack', gdb.COMMAND_USER)
+		
+	def is_in(self,arg,list):
+		for i in list:
+			if arg == i:
+				return True;
+		return False
+	
+	def find_tcb_list(self,dq_entry_t):
+		tcb_list = []
+		tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer())
+		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]):
+			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]
+	
+	def getTCB(self):
+		list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks']
+		tcb_list = [];
+		for l in list_of_listsnames:
+			li = gdb.lookup_global_symbol(l)
+			cursor = li.value()['head']
+			tcb_list = tcb_list + self.find_tcb_list(cursor)
+		return tcb_list
+		
+	def getSPfromTask(self,tcb):
+		regmap = NX_register_set.v7em_regmap
+		a =tcb['xcp']['regs']
+		return 	int(a[regmap['SP']])
+	
+	def find_closest(self,list,val):
+		tmp_list = [abs(i-val) for i in list]
+		tmp_min = min(tmp_list)
+		idx = tmp_list.index(tmp_min)
+		return idx,list[idx]
+	
+	def find_next_stack(self,address,_dict_in):
+		add_list = []
+		name_list = []
+		for key in _dict_in.keys():
+			for i in range(3):
+				if _dict_in[key][i] < address:
+					add_list.append(_dict_in[key][i])
+					if i == 2: # the last one is the processes stack pointer
+						name_list.append(self.check_name(key)+"_SP")
+					else:
+						name_list.append(self.check_name(key))
+					
+		idx,new_address = self.find_closest(add_list,address)
+		return new_address,name_list[idx]
+	
+	def check_name(self,name):
+		if isinstance(name,(list)):
+			name = name[0];
+		idx = name.find("\\")
+		newname = name[:idx]
+		
+		return newname
+	 
+	def invoke(self,args,sth):
+		tcb = self.getTCB();
+		stackadresses={};
+		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(self.getSPfromTask(t))
+			stackadresses[str(t['name'])] = p;
+		address = int("0x30000000",0)
+		print("stack address  :  process")
+		for i in range(len(stackadresses)*3):
+			  address,name = self.find_next_stack(address,stackadresses)
+			  print(hex(address),": ",name)
+
+NX_check_stack_order()
+					 
+class NX_run_debug_util(gdb.Command):
+	""" show the registers of a task corresponding to a tcb address"""
+	def __init__(self):
+		super(NX_run_debug_util,self).__init__('show regs', gdb.COMMAND_USER)
+	
+	def printRegisters(self,task):
+		regmap = NX_register_set.v7em_regmap
+		a =task._tcb['xcp']['regs']
+		print("relevant registers in ",task.name,":")
+		for reg in regmap:
+			hex_addr= hex(int(a[regmap[reg]]))
+			eval_string = 'info line *'+str(hex_addr)
+			print(reg,": ",hex_addr,)
+			
+	def getPCfromTask(self,task):
+		regmap = NX_register_set.v7em_regmap
+		a =task._tcb['xcp']['regs']
+		return 	hex(int(a[regmap['PC']]))
+	
+	def invoke(self,args,sth):
+		tasks = NX_task.tasks() 
+		if args == '':
+			for t in tasks:
+				self.printRegisters(t)
+				eval_str = "list *"+str(self.getPCfromTask(t))
+				print("this is the location in code where the current threads $pc is:")
+				gdb.execute(eval_str)
+		else:
+			tcb_nr = int(args);
+			print("tcb_nr = ",tcb_nr)
+			t = tasks[tcb_nr]
+			self.printRegisters(t)
+			eval_str = "list *"+str(self.getPCfromTask(t))
+			print("this is the location in code where the current threads $pc is:")
+			gdb.execute(eval_str)
+			
+NX_run_debug_util()
+
+		
+class NX_search_tcb(gdb.Command):
+	""" shot PID's of all running tasks """
+	
+	def __init__(self):
+		super(NX_search_tcb,self).__init__('show alltcb', gdb.COMMAND_USER)
+	
+	def is_in(self,arg,list):
+		for i in list:
+			if arg == i:
+				return True;
+		return False
+	
+	def find_tcb_list(self,dq_entry_t):
+		tcb_list = []
+		tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer())
+		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]):
+			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]
+	
+	def invoke(self,args,sth):
+		list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks']
+		tasks = [];
+		for l in list_of_listsnames:
+			li = gdb.lookup_global_symbol(l)
+			cursor = li.value()['head']
+			tasks = tasks + self.find_tcb_list(cursor)
+		
+		# filter for tasks that are listed twice
+		tasks_filt = {}
+		for t in tasks:
+			pid = 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)))
+		for pid in tasks_filt.keys():
+			print("PID: ",pid," ",tasks_filt[pid])
+
+NX_search_tcb()
+
+
+class NX_my_bt(gdb.Command):
+	""" 'fake' backtrace: backtrace the stack of a process and check every suspicious address for the list 
+	arg: tcb_address$
+	(can easily be found by typing 'showtask').
+	"""
+	
+	def __init__(self):
+		super(NX_my_bt,self).__init__('show mybt', gdb.COMMAND_USER)
+		
+	def readmem(self,addr):
+		'''
+		read memory at addr and return nr
+		'''	
+		str_to_eval = "x/x "+hex(addr)
+		resp = gdb.execute(str_to_eval,to_string = True)
+		idx = resp.find('\t')
+		return int(resp[idx:],16)
+	
+	def is_in_bounds(self,val):
+		lower_bound = int("08004000",16)
+		upper_bound = int("080ae0c0",16);
+		#print(lower_bound," ",val," ",upper_bound)
+		if val>lower_bound and val<upper_bound:
+			return True;
+		else:
+			return False;
+	def get_tcb_from_address(self,addr):
+		addr_value = gdb.Value(addr)
+		tcb_ptr = addr_value.cast(gdb.lookup_type('struct tcb_s').pointer())
+		return tcb_ptr.dereference()
+	
+	def print_instruction_at(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
+			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
+			
+			
+			
+		
+	def invoke(self,args,sth):
+		addr_dec = int(args[2:],16)
+		_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
+		
+		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):
+			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
+		
+NX_my_bt()

ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil

@@ -1,14 +1,10 @@
 #!nsh
 #
-# HILStar / X-Plane
-#
-# Lorenz Meier <lm@inf.ethz.ch>
+# HILStar
+# <lorenz@px4.io>
 #
 
 sh /etc/init.d/rc.fw_defaults
 
-echo "X Plane HIL starting.."
-
 set HIL yes
-
 set MIXER FMU_AERT

ROMFS/px4fmu_common/init.d/10015_tbs_discovery

@@ -2,7 +2,7 @@
 #
 # Team Blacksheep Discovery Quadcopter
 #
-# Anton Babushkin <anton.babushkin@me.com>, Simon Wilks <sjwilks@gmail.com>, Thomas Gubler <thomasgubler@gmail.com>
+# Anton Babushkin <anton@px4.io>, Simon Wilks <simon@px4.io>
 #
 
 sh /etc/init.d/rc.mc_defaults

ROMFS/px4fmu_common/init.d/10016_3dr_iris

@@ -2,7 +2,7 @@
 #
 # 3DR Iris Quadcopter
 #
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
 #
 
 sh /etc/init.d/rc.mc_defaults

ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d

@@ -2,8 +2,7 @@
 #
 # Steadidrone QU4D
 #
-# Thomas Gubler <thomasgubler@gmail.com>
-# Lorenz Meier <lm@inf.ethz.ch>
+# Thomas Gubler <thomas@px4.io>
 #
 
 sh /etc/init.d/rc.mc_defaults

ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil

@@ -2,7 +2,7 @@
 #
 # HIL Quadcopter X
 #
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
 #
 
 sh /etc/init.d/rc.mc_defaults

ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil

@@ -2,7 +2,7 @@
 #
 # HIL Quadcopter +
 #
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
 #
 
 sh /etc/init.d/rc.mc_defaults

ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil

@@ -2,13 +2,11 @@
 #
 # HIL Rascal 110 (Flightgear)
 #
-# Thomas Gubler <thomasgubler@gmail.com>
+# Thomas Gubler <thomas@px4.io>
 #
 
 sh /etc/init.d/rc.fw_defaults
 
-echo "HIL Rascal 110 starting.."
-
 set HIL yes
 
 set MIXER FMU_AERT

ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil

@@ -2,7 +2,7 @@
 #
 # HIL Malolo 1 (Flightgear)
 #
-# Thomas Gubler <thomasgubler@gmail.com>
+# Thomas Gubler <thomas@px4.io>
 #
 
 sh /etc/init.d/rc.fw_defaults

ROMFS/px4fmu_common/init.d/11001_hexa_cox

@@ -2,12 +2,12 @@
 #
 # Generic 10" Hexa coaxial geometry
 #
-# Lorenz Meier <lm@inf.ethz.ch>
+# Lorenz Meier <lorenz@px4.io>
 #
 
 sh /etc/init.d/rc.mc_defaults
 
 set MIXER FMU_hexa_cox
 
-# We only can run one channel group with one rate, so set all 8 channels
+# Need to set all 8 channels
 set PWM_OUTPUTS 12345678

ROMFS/px4fmu_common/init.d/12001_octo_cox

@@ -2,7 +2,7 @@
 #
 # Generic 10" Octo coaxial geometry
 #
-# Lorenz Meier <lm@inf.ethz.ch>
+# Lorenz Meier <lorenz@px4.io>
 #
 
 sh /etc/init.d/rc.mc_defaults

ROMFS/px4fmu_common/init.d/3030_io_camflyer

@@ -3,3 +3,6 @@
 sh /etc/init.d/rc.fw_defaults
 
 set MIXER FMU_Q
+# Provide ESC a constant 1000 us pulse while disarmed
+set PWM_OUTPUTS 4
+set PWM_DISARMED 1000

ROMFS/px4fmu_common/init.d/3031_phantom

@@ -2,7 +2,7 @@
 #
 # Phantom FPV Flying Wing
 #
-# Simon Wilks <sjwilks@gmail.com>, Thomas Gubler <thomasgubler@gmail.com>
+# Simon Wilks <simon@px4.io>
 #
 
 sh /etc/init.d/rc.fw_defaults
@@ -21,8 +21,6 @@ then
 	param set FW_PR_P 0.03
 	param set FW_P_LIM_MAX 50
 	param set FW_P_LIM_MIN -50
-	param set FW_P_RMAX_NEG 0
-	param set FW_P_RMAX_POS 0
 	param set FW_P_ROLLFF 1
 	param set FW_RR_FF 0.5
 	param set FW_RR_I 0.02

ROMFS/px4fmu_common/init.d/3032_skywalker_x5

@@ -2,7 +2,7 @@
 #
 # Skywalker X5 Flying Wing
 #
-# Thomas Gubler <thomasgubler@gmail.com>, Julian Oes <joes@student.ethz.ch>
+# Thomas Gubler <thomas@px4.io>, Julian Oes <julian@px4.io>
 #
 
 sh /etc/init.d/rc.fw_defaults
@@ -19,10 +19,6 @@ then
 	param set FW_PR_I 0
 	param set FW_PR_IMAX 0.2
 	param set FW_PR_P 0.03
-	param set FW_P_LIM_MAX 45
-	param set FW_P_LIM_MIN -45
-	param set FW_P_RMAX_NEG 0
-	param set FW_P_RMAX_POS 0
 	param set FW_P_ROLLFF 1
 	param set FW_RR_FF 0.3
 	param set FW_RR_I 0

ROMFS/px4fmu_common/init.d/3033_wingwing

@@ -2,7 +2,7 @@
 #
 # Wing Wing (aka Z-84) Flying Wing
 #
-# Simon Wilks <sjwilks@gmail.com>
+# Simon Wilks <simon@px4.io>
 #
 
 sh /etc/init.d/rc.fw_defaults

ROMFS/px4fmu_common/init.d/3034_fx79

@@ -2,7 +2,7 @@
 #
 # FX-79 Buffalo Flying Wing
 #
-# Simon Wilks <sjwilks@gmail.com>
+# Simon Wilks <simon@px4.io>
 #
 
 sh /etc/init.d/rc.fw_defaults

ROMFS/px4fmu_common/init.d/3035_viper

@@ -2,7 +2,7 @@
 #
 # Viper
 #
-# Simon Wilks <sjwilks@gmail.com>
+# Simon Wilks <simon@px4.io>
 #
 
 sh /etc/init.d/rc.fw_defaults

ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha

@@ -1,8 +1,8 @@
 #!nsh
 #
-# TBS Caipirinha Flying Wing
+# TBS Caipirinha
 #
-# Thomas Gubler <thomasgubler@gmail.com>
+# Thomas Gubler <thomas@px4.io>
 #
 
 sh /etc/init.d/rc.fw_defaults
@@ -22,10 +22,6 @@ then
 	param set FW_PR_I 0
 	param set FW_PR_IMAX 0.2
 	param set FW_PR_P 0.03
-	param set FW_P_LIM_MAX 45
-	param set FW_P_LIM_MIN -45
-	param set FW_P_RMAX_NEG 0
-	param set FW_P_RMAX_POS 0
 	param set FW_P_ROLLFF 0
 	param set FW_RR_FF 0.3
 	param set FW_RR_I 0

ROMFS/px4fmu_common/init.d/4001_quad_x

@@ -2,7 +2,7 @@
 #
 # Generic 10" Quad X geometry
 #
-# Lorenz Meier <lm@inf.ethz.ch>
+# Lorenz Meier <lorenz@px4.io>
 #
 
 sh /etc/init.d/rc.mc_defaults

ROMFS/px4fmu_common/init.d/4008_ardrone

@@ -3,9 +3,6 @@
 # ARDrone
 #
 
-echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board"
-
-# Just use the default multicopter settings.
 sh /etc/init.d/rc.mc_defaults
 
 #

ROMFS/px4fmu_common/init.d/4010_dji_f330

@@ -1,8 +1,8 @@
 #!nsh
 #
-# DJI Flame Wheel F330 Quadcopter
+# DJI Flame Wheel F330
 #
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
 #
 
 sh /etc/init.d/4001_quad_x

ROMFS/px4fmu_common/init.d/4011_dji_f450

@@ -1,8 +1,8 @@
 #!nsh
 #
-# DJI Flame Wheel F450 Quadcopter
+# DJI Flame Wheel F450
 #
-# Lorenz Meier <lm@inf.ethz.ch>
+# Lorenz Meier <lorenz@px4.io>
 #
 
 sh /etc/init.d/4001_quad_x

ROMFS/px4fmu_common/init.d/4012_quad_x_can

@@ -2,7 +2,7 @@
 #
 # F450-sized quadrotor with CAN
 #
-# Lorenz Meier <lm@inf.ethz.ch>
+# Pavel Kirienko <pavel@px4.io>
 #
 
 sh /etc/init.d/4001_quad_x

ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb

@@ -3,7 +3,7 @@
 # Hobbyking Micro Integrated PCB Quadcopter
 # with SimonK ESC firmware and Mystery A1510 motors
 #
-# Thomas Gubler <thomasgubler@gmail.com>
+# Thomas Gubler <thomas@px4.io>
 #
 echo "HK Micro PCB Quad"
 

ROMFS/px4fmu_common/init.d/5001_quad_+

@@ -2,7 +2,7 @@
 #
 # Generic 10" Quad + geometry
 #
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
 #
 
 sh /etc/init.d/rc.mc_defaults

ROMFS/px4fmu_common/init.d/6001_hexa_x

@@ -2,12 +2,12 @@
 #
 # Generic 10" Hexa X geometry
 #
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
 #
 
 sh /etc/init.d/rc.mc_defaults
 
 set MIXER FMU_hexa_x
 
-# We only can run one channel group with one rate, so set all 8 channels
+# Need to set all 8 channels
 set PWM_OUTPUTS 12345678

ROMFS/px4fmu_common/init.d/7001_hexa_+

@@ -2,12 +2,12 @@
 #
 # Generic 10" Hexa + geometry
 #
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
 #
 
 sh /etc/init.d/rc.mc_defaults
 
 set MIXER FMU_hexa_+
 
-# We only can run one channel group with one rate, so set all 8 channels
+# Need to set all 8 channels
 set PWM_OUTPUTS 12345678

ROMFS/px4fmu_common/init.d/8001_octo_x

@@ -2,7 +2,7 @@
 #
 # Generic 10" Octo X geometry
 #
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
 #
 
 sh /etc/init.d/rc.mc_defaults

ROMFS/px4fmu_common/init.d/9001_octo_+

@@ -2,7 +2,7 @@
 #
 # Generic 10" Octo + geometry
 #
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
 #
 
 sh /etc/init.d/rc.mc_defaults

ROMFS/px4fmu_common/init.d/rc.autostart

@@ -1,5 +1,4 @@
 #
-# Check if auto-setup from one of the standard scripts is wanted
 # SYS_AUTOSTART = 0 means no autostart (default)
 #
 # AUTOSTART PARTITION:
@@ -18,7 +17,7 @@
 # 12000	..	12999		Octo Cox
 
 #
-# Simulation setups
+# Simulation
 #
 
 if param compare SYS_AUTOSTART 901
@@ -53,7 +52,7 @@ then
 fi
 
 #
-# Standard plane
+# Plane
 #
 
 if param compare SYS_AUTOSTART 2100 100

ROMFS/px4fmu_common/init.d/rc.interface

@@ -10,7 +10,7 @@ then
 	#
 	set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix
 	
-	#Use the mixer file from the SD-card if it exists
+	# Use the mixer file from the SD-card if it exists
 	if [ -f $MIXERSD ]
 	then
 		set MIXER_FILE $MIXERSD
@@ -54,7 +54,6 @@ then
 		#
 		if [ $PWM_RATE != none ]
 		then
-			echo "[init] Set PWM rate: $PWM_RATE"
 			pwm rate -c $PWM_OUTPUTS -r $PWM_RATE
 		fi
 		
@@ -63,17 +62,14 @@ then
 		#
 		if [ $PWM_DISARMED != none ]
 		then
-			echo "[init] Set PWM disarmed: $PWM_DISARMED"
 			pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED
 		fi
 		if [ $PWM_MIN != none ]
 		then
-			echo "[init] Set PWM min: $PWM_MIN"
 			pwm min -c $PWM_OUTPUTS -p $PWM_MIN
 		fi
 		if [ $PWM_MAX != none ]
 		then
-			echo "[init] Set PWM max: $PWM_MAX"
 			pwm max -c $PWM_OUTPUTS -p $PWM_MAX
 		fi
 	fi

ROMFS/px4fmu_common/init.d/rc.jig

@@ -1,10 +0,0 @@
-#!nsh
-#
-# Test jig startup script
-#
-
-echo "[testing] doing production test.."
-
-tests jig
-
-echo "[testing] testing done"

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -54,7 +54,6 @@ then
 	fi
 fi
 
-# Start airspeed sensors
 if meas_airspeed start
 then
 	echo "[init] Using MEAS airspeed sensor"
@@ -68,16 +67,12 @@ else
 	fi
 fi
 
-# Check for flow sensor
 if px4flow start
 then
 fi
 
 #
-# Start the sensor collection task.
-# IMPORTANT: this also loads param offsets
-# ALWAYS start this task before the
-# preflight_check.
+# Start sensors -> preflight_check
 #
 if sensors start
 then

ROMFS/px4fmu_common/init.d/rcS

@@ -137,9 +137,7 @@ then
 	#
 	if param compare SYS_AUTOCONFIG 1
 	then
-		# We can't be sure the defaults haven't changed, so
-		# if someone requests a re-configuration, we do it
-		# cleanly from scratch (except autostart / autoconfig)
+		# Wipe out params
 		param reset_nostart
 		set DO_AUTOCONFIG yes
 	else
@@ -202,12 +200,10 @@ then
 
 		if px4io checkcrc $IO_FILE
 		then
-			echo "[init] PX4IO CRC OK"
 			echo "PX4IO CRC OK" >> $LOG_FILE
 
 			set IO_PRESENT yes
 		else
-			echo "[init] Trying to update"
 			echo "PX4IO Trying to update" >> $LOG_FILE
 
 			tone_alarm MLL32CP8MB
@@ -217,18 +213,15 @@ then
 				usleep 500000
 				if px4io checkcrc $IO_FILE
 				then
-					echo "[init] PX4IO CRC OK, update successful"
 					echo "PX4IO CRC OK after updating" >> $LOG_FILE
 					tone_alarm MLL8CDE
 
 					set IO_PRESENT yes
 				else
-					echo "[init] ERROR: PX4IO update failed"
 					echo "PX4IO update failed" >> $LOG_FILE
 					tone_alarm $TUNE_OUT_ERROR
 				fi
 			else
-				echo "[init] ERROR: PX4IO update failed"
 				echo "PX4IO update failed" >> $LOG_FILE
 				tone_alarm $TUNE_OUT_ERROR
 			fi
@@ -281,16 +274,12 @@ then
 		fi
 	fi
 
-	#
-	# Start the datamanager (and do not abort boot if it fails)
-	#
+	# waypoint storage
 	if dataman start
 	then
 	fi
 
-	#
-	# Start the Commander (needs to be this early for in-air-restarts)
-	#
+	# Needs to be this early for in-air-restarts
 	commander start
 
 	#
@@ -424,9 +413,6 @@ then
 		fi
 	fi
 
-	#
-	# MAVLink
-	#
 	if [ $MAVLINK_FLAGS == default ]
 	then
 		# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
@@ -454,15 +440,10 @@ then
 	# Sensors, Logging, GPS
 	#
 	sh /etc/init.d/rc.sensors
-
-	#
-	# Start logging in all modes, including HIL
-	#
 	sh /etc/init.d/rc.logging
 
 	if [ $GPS == yes ]
 	then
-		echo "[init] Start GPS"
 		if [ $GPS_FAKE == yes ]
 		then
 			echo "[init] Faking GPS"

Tools/px_mkfw.py

@@ -73,6 +73,7 @@ parser.add_argument("--version",	action="store", help="set a version string")
 parser.add_argument("--summary",	action="store", help="set a brief description")
 parser.add_argument("--description",	action="store", help="set a longer description")
 parser.add_argument("--git_identity",	action="store", help="the working directory to check for git identity")
+parser.add_argument("--parameter_xml",	action="store", help="the parameters.xml file")
 parser.add_argument("--image",		action="store", help="the firmware image")
 args = parser.parse_args()
 
@@ -101,6 +102,10 @@ if args.git_identity != None:
 	p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout
 	desc['git_identity']	= str(p.read().strip())
 	p.close()
+if args.parameter_xml != None:
+	f = open(args.parameter_xml, "rb")
+	bytes = f.read()
+	desc['parameter_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8')
 if args.image != None:
 	f = open(args.image, "rb")
 	bytes = f.read()

Tools/px_uploader.py

@@ -458,7 +458,7 @@ if os.path.exists("/usr/sbin/ModemManager"):
 
 # Load the firmware file
 fw = firmware(args.firmware)
-print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))
+print("Loaded firmware for %x,%x, size: %d bytes, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size')))
 print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.")
 
 # Spin waiting for a device to show up

Tools/upload.sh

@@ -0,0 +1,28 @@
+#!/bin/bash
+
+EXEDIR=`pwd`
+BASEDIR=$(dirname $0)
+
+SYSTYPE=`uname -s`
+
+#
+# Serial port defaults.
+#
+# XXX The uploader should be smarter than this.
+#
+if [ $SYSTYPE = "Darwin" ];
+then
+SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*"
+fi
+
+if [ $SYSTYPE = "Linux" ];
+then
+SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*"
+fi
+
+if [ $SYSTYPE = "" ];
+then
+SERIAL_PORTS="COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0"
+fi
+
+python $BASEDIR/px_uploader.py --port $SERIAL_PORTS $1

makefiles/config_px4fmu-v1_default.mk

@@ -132,6 +132,9 @@ MODULES		+= lib/launchdetection
 # Hardware test
 #MODULES			+= examples/hwtest
 
+# Generate parameter XML file
+GEN_PARAM_XML = 1
+
 #
 # Transitional support - add commands from the NuttX export archive.
 #

makefiles/config_px4fmu-v2_default.mk

@@ -40,14 +40,8 @@ MODULES		+= drivers/meas_airspeed
 MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors
 MODULES		+= drivers/mkblctrl
-MODULES		+= drivers/pca8574
 MODULES		+= drivers/px4flow
 
-
-# Needs to be burned to the ground and re-written; for now,
-# just don't build it.
-#MODULES		+= drivers/mkblctrl
-
 #
 # System commands
 #
@@ -61,7 +55,6 @@ MODULES		+= systemcmds/pwm
 MODULES		+= systemcmds/esc_calib
 MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/top
-MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
@@ -81,10 +74,8 @@ MODULES		+= modules/uavcan
 # Estimation modules (EKF/ SO3 / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
-MODULES		+= modules/attitude_estimator_so3
 MODULES		+= modules/ekf_att_pos_estimator
 MODULES		+= modules/position_estimator_inav
-MODULES		+= examples/flow_position_estimator
 
 #
 # Vehicle Control
@@ -100,12 +91,6 @@ MODULES		+= modules/mc_pos_control
 #
 MODULES		+= modules/sdlog2
 
-#
-# Unit tests
-#
-#MODULES 	+= modules/unit_test
-#MODULES 	+= modules/commander/commander_tests
-
 #
 # Library modules
 #
@@ -139,7 +124,7 @@ MODULES		+= modules/bottle_drop
 #MODULES		+= examples/math_demo
 # Tutorial code from
 # https://pixhawk.ethz.ch/px4/dev/hello_sky
-MODULES		+= examples/px4_simple_app
+#MODULES		+= examples/px4_simple_app
 
 # Tutorial code from
 # https://pixhawk.ethz.ch/px4/dev/daemon
@@ -156,6 +141,9 @@ MODULES		+= examples/px4_simple_app
 # Hardware test
 #MODULES			+= examples/hwtest
 
+# Generate parameter XML file
+GEN_PARAM_XML = 1
+
 #
 # Transitional support - add commands from the NuttX export archive.
 #

makefiles/config_px4fmu-v2_test.mk

@@ -53,7 +53,14 @@ LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= platforms/nuttx
 
 #
-# Tests
+# Modules to test-build, but not useful for test environment
+#
+MODULES		+= modules/attitude_estimator_so3
+MODULES		+= drivers/pca8574
+MODULES		+= examples/flow_position_estimator
+
+#
+# Libraries
 #
 MODULES 	+= modules/unit_test
 MODULES		+= modules/mavlink/mavlink_tests

makefiles/firmware.mk

@@ -467,6 +467,7 @@ endif
 PRODUCT_BUNDLE		 = $(WORK_DIR)firmware.px4
 PRODUCT_BIN		 = $(WORK_DIR)firmware.bin
 PRODUCT_ELF		 = $(WORK_DIR)firmware.elf
+PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml
 
 .PHONY:			firmware
 firmware:		$(PRODUCT_BUNDLE)
@@ -497,9 +498,17 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS)
 
 $(PRODUCT_BUNDLE):	$(PRODUCT_BIN)
 	@$(ECHO) %% Generating $@
+ifdef GEN_PARAM_XML
+	python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml
+	$(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \
+		--git_identity $(PX4_BASE) \
+		--parameter_xml $(PRODUCT_PARAMXML) \
+		--image $< > $@
+else
 	$(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \
 		--git_identity $(PX4_BASE) \
 		--image $< > $@
+endif
 
 $(PRODUCT_BIN):		$(PRODUCT_ELF)
 	$(call SYM_TO_BIN,$<,$@)

makefiles/toolchain_gnu-arm-eabi.mk

@@ -50,11 +50,11 @@ OBJDUMP			 = $(CROSSDEV)objdump
 
 # Check if the right version of the toolchain is available
 #
-CROSSDEV_VER_SUPPORTED	 = 4.7
+CROSSDEV_VER_SUPPORTED	 = 4.7.4 4.7.5 4.7.6 4.8.4
 CROSSDEV_VER_FOUND	 = $(shell $(CC) -dumpversion)
 
-ifeq (,$(findstring $(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND)))
-$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED).x)
+ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED)))
+$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED))
 endif
 
 

nuttx-configs/px4fmu-v1/nsh/Make.defs

@@ -53,7 +53,7 @@ NM			 = $(CROSSDEV)nm
 OBJCOPY			 = $(CROSSDEV)objcopy
 OBJDUMP			 = $(CROSSDEV)objdump
 
-MAXOPTIMIZATION		 = -O3
+MAXOPTIMIZATION		 = -Os
 ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
 			   -mthumb \
 			   -march=armv7e-m \

nuttx-configs/px4fmu-v2/nsh/Make.defs

@@ -53,7 +53,7 @@ NM			 = $(CROSSDEV)nm
 OBJCOPY			 = $(CROSSDEV)objcopy
 OBJDUMP			 = $(CROSSDEV)objdump
 
-MAXOPTIMIZATION		 = -O3
+MAXOPTIMIZATION		 = -Os
 ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
 			   -mthumb \
 			   -march=armv7e-m \

nuttx-configs/px4fmu-v2/scripts/ld.script

@@ -50,7 +50,8 @@
 
 MEMORY
 {
-    flash (rx)   : ORIGIN = 0x08004000, LENGTH = 2032K
+    /* disabled due to silicon errata flash (rx)   : ORIGIN = 0x08004000, LENGTH = 2032K */
+    flash (rx)   : ORIGIN = 0x08004000, LENGTH = 1008K
     sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 192K
     ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
 }

nuttx-configs/px4io-v1/nsh/Make.defs

@@ -53,7 +53,7 @@ NM			 = $(CROSSDEV)nm
 OBJCOPY			 = $(CROSSDEV)objcopy
 OBJDUMP			 = $(CROSSDEV)objdump
 
-MAXOPTIMIZATION		 = -O3
+MAXOPTIMIZATION		 = -Os
 ARCHCPUFLAGS		 = -mcpu=cortex-m3 \
 			   -mthumb \
 			   -march=armv7-m

nuttx-configs/px4io-v2/nsh/Make.defs

@@ -53,15 +53,11 @@ NM			 = $(CROSSDEV)nm
 OBJCOPY			 = $(CROSSDEV)objcopy
 OBJDUMP			 = $(CROSSDEV)objdump
 
-MAXOPTIMIZATION		 = -O3
+MAXOPTIMIZATION		 = -Os
 ARCHCPUFLAGS		 = -mcpu=cortex-m3 \
 			   -mthumb \
 			   -march=armv7-m
 
-# enable precise stack overflow tracking
-#INSTRUMENTATIONDEFINES	 = -finstrument-functions \
-#			   -ffixed-r10
-
 # use our linker script
 LDSCRIPT		 = ld.script
 

src/drivers/airspeed/airspeed.cpp

@@ -147,7 +147,7 @@ Airspeed::init()
 		_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
 
 		if (_airspeed_pub < 0)
-			warnx("failed to create airspeed sensor object. uORB started?");
+			warnx("uORB started?");
 	}
 
 	ret = OK;

src/drivers/ardrone_interface/ardrone_interface.c

@@ -89,8 +89,8 @@ static void
 usage(const char *reason)
 {
 	if (reason)
-		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d <UART>]\n\n");
+		warnx("%s\n", reason);
+	warnx("usage: {start|stop|status} [-d <UART>]\n\n");
 	exit(1);
 }
 
@@ -110,7 +110,7 @@ int ardrone_interface_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("ardrone_interface already running\n");
+			warnx("already running\n");
 			/* this is not an error */
 			exit(0);
 		}
@@ -132,9 +132,9 @@ int ardrone_interface_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tardrone_interface is running\n");
+			warnx("running");
 		} else {
-			printf("\tardrone_interface not started\n");
+			warnx("not started");
 		}
 		exit(0);
 	}
@@ -158,7 +158,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
 
 	/* Back up the original uart configuration to restore it after exit */
 	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
+		warnx("ERR: TCGETATTR %s: %d", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
@@ -171,14 +171,14 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
 
 	/* Set baud rate */
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+		warnx("ERR: cfsetispeed %s: %d", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
 
 
 	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+		warnx("ERR: tcsetattr: %s", uart_name);
 		close(uart);
 		return -1;
 	}
@@ -192,9 +192,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 
 	char *device = "/dev/ttyS1";
 
-	/* welcome user */
-	printf("[ardrone_interface] Control started, taking over motors\n");
-
 	/* File descriptors */
 	int gpios;
 
@@ -237,7 +234,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	struct termios uart_config_original;
 
 	if (motor_test_mode) {
-		printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
+		warnx("setting 10 %% thrust.\n");
 	}
 
 	/* Led animation */
@@ -255,9 +252,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
-	printf("[ardrone_interface] Motors initialized - ready.\n");
-	fflush(stdout);
-
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
 	ardrone_write = ardrone_open_uart(device, &uart_config_original);
 
@@ -265,7 +259,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	gpios = ar_multiplexing_init();
 
 	if (ardrone_write < 0) {
-		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
+		warnx("No UART, exiting.");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -273,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	/* initialize motors */
 	if (OK != ar_init_motors(ardrone_write, gpios)) {
 		close(ardrone_write);
-		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
+		warnx("motor init fail");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -294,7 +288,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	gpios = ar_multiplexing_init();
 
 	if (ardrone_write < 0) {
-		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
+		warnx("write fail");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -302,7 +296,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	/* initialize motors */
 	if (OK != ar_init_motors(ardrone_write, gpios)) {
 		close(ardrone_write);
-		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
+		warnx("motor init fail");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -378,11 +372,9 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	int termios_state;
 
 	if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
+		warnx("ERR: tcsetattr");
 	}
 
-	printf("[ardrone_interface] Restored original UART config, exiting..\n");
-
 	/* close uarts */
 	close(ardrone_write);
 	ar_multiplexing_deinit(gpios);

src/drivers/ardrone_interface/ardrone_motor_control.c

@@ -301,7 +301,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
 	ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
 
 	if (errcounter != 0) {
-		fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
+		warnx("Failed %d times", -errcounter);
 		fflush(stdout);
 	}
 	return errcounter;

src/drivers/boards/aerocore/module.mk

@@ -6,3 +6,5 @@ SRCS		 = aerocore_init.c \
 		   aerocore_pwm_servo.c \
 		   aerocore_spi.c \
 		   aerocore_led.c
+
+MAXOPTIMIZATION	 = -Os

src/drivers/boards/px4fmu-v1/module.mk

@@ -8,3 +8,5 @@ SRCS		 = px4fmu_can.c \
 		   px4fmu_spi.c \
 		   px4fmu_usb.c \
 		   px4fmu_led.c
+
+MAXOPTIMIZATION	 = -Os

src/drivers/boards/px4fmu-v2/module.mk

@@ -8,3 +8,5 @@ SRCS		 = px4fmu_can.c \
 		   px4fmu_spi.c \
 		   px4fmu_usb.c \
 		   px4fmu2_led.c
+
+MAXOPTIMIZATION	 = -Os

src/drivers/boards/px4io-v1/module.mk

@@ -4,3 +4,5 @@
 
 SRCS	= px4io_init.c \
 	  px4io_pwm_servo.c
+
+MAXOPTIMIZATION	 = -Os

src/drivers/boards/px4io-v2/module.mk

@@ -4,3 +4,5 @@
 
 SRCS		= px4iov2_init.c \
 		  px4iov2_pwm_servo.c
+
+MAXOPTIMIZATION	 = -Os

src/drivers/device/device.h

@@ -130,7 +130,8 @@ public:
 	enum DeviceBusType {
 		DeviceBusType_UNKNOWN = 0,
 		DeviceBusType_I2C     = 1,
-		DeviceBusType_SPI     = 2
+		DeviceBusType_SPI     = 2,
+		DeviceBusType_UAVCAN  = 3,
 	};
 
 	/*

src/drivers/drv_pwm_output.h

@@ -117,6 +117,23 @@ struct pwm_output_values {
 	unsigned			channel_count;
 };
 
+
+/**
+ * RC config values for a channel
+ *
+ * This allows for PX4IO_PAGE_RC_CONFIG values to be set without a
+ * param_get() dependency
+ */
+struct pwm_output_rc_config {
+	uint8_t channel;
+	uint16_t rc_min;
+	uint16_t rc_trim;
+	uint16_t rc_max;
+	uint16_t rc_dz;
+	uint16_t rc_assignment;
+	bool     rc_reverse;
+};
+
 /*
  * ORB tag for PWM outputs.
  */
@@ -214,7 +231,19 @@ ORB_DECLARE(output_pwm);
 #define PWM_SERVO_SET_TERMINATION_FAILSAFE	_IOC(_PWM_SERVO_BASE, 25)
 
 /** force safety switch on (to enable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_ON  _IOC(_PWM_SERVO_BASE, 26)
+#define PWM_SERVO_SET_FORCE_SAFETY_ON		_IOC(_PWM_SERVO_BASE, 26)
+
+/** set RC config for a channel. This takes a pointer to pwm_output_rc_config */
+#define PWM_SERVO_SET_RC_CONFIG			_IOC(_PWM_SERVO_BASE, 27)
+
+/** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
+#define PWM_SERVO_SET_OVERRIDE_OK		_IOC(_PWM_SERVO_BASE, 28)
+
+/** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
+#define PWM_SERVO_CLEAR_OVERRIDE_OK		_IOC(_PWM_SERVO_BASE, 29)
+
+/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
+#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE	_IOC(_PWM_SERVO_BASE, 30)
 
 /*
  *

src/drivers/frsky_telemetry/frsky_telemetry.c

@@ -84,7 +84,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
 	/* Back up the original UART configuration to restore it after exit */
 	int termios_state;
 	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
+		warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
@@ -100,13 +100,13 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
 	static const speed_t speed = B9600;
 
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-		warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+		warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
 
 	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+		warnx("ERR: %s (tcsetattr)\n", uart_name);
 		close(uart);
 		return -1;
 	}
@@ -151,9 +151,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 		}
 	}
 
-	/* Print welcome text */
-	warnx("FrSky telemetry interface starting...");
-
 	/* Open UART */
 	struct termios uart_config_original;
 	const int uart = frsky_open_uart(device_name, &uart_config_original);

src/drivers/gps/gps.cpp

@@ -274,7 +274,6 @@ GPS::task_main_trampoline(void *arg)
 void
 GPS::task_main()
 {
-	log("starting");
 
 	/* open the serial port */
 	_serial_fd = ::open(_port, O_RDWR);

src/drivers/hil/hil.cpp

@@ -442,8 +442,6 @@ HIL::task_main()
 	/* make sure servos are off */
 	// up_pwm_servo_deinit();
 
-	log("stopping");
-
 	/* note - someone else is responsible for restoring the GPIO config */
 
 	/* tell the dtor that we are exiting */

src/drivers/hmc5883/hmc5883.cpp

@@ -1049,11 +1049,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	 * LSM/Ga, giving 1.16 and 1.08 */
 	float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
 
-	warnx("starting mag scale calibration");
-
 	/* start the sensor polling at 50 Hz */
 	if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
-		warn("failed to set 2Hz poll rate");
+		warn("FAILED: SENSORIOCSPOLLRATE 2Hz");
 		ret = 1;
 		goto out;
 	}
@@ -1061,25 +1059,25 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	/* Set to 2.5 Gauss. We ask for 3 to get the right part of
          * the chained if statement above. */
 	if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
-		warnx("failed to set 2.5 Ga range");
+		warnx("FAILED: MAGIOCSRANGE 3.3 Ga");
 		ret = 1;
 		goto out;
 	}
 
 	if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
-		warnx("failed to enable sensor calibration mode");
+		warnx("FAILED: MAGIOCEXSTRAP 1");
 		ret = 1;
 		goto out;
 	}
 
 	if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
-		warn("WARNING: failed to get scale / offsets for mag");
+		warn("FAILED: MAGIOCGSCALE 1");
 		ret = 1;
 		goto out;
 	}
 
 	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
-		warn("WARNING: failed to set null scale / offsets for mag");
+		warn("FAILED: MAGIOCSSCALE 1");
 		ret = 1;
 		goto out;
 	}
@@ -1094,7 +1092,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		ret = ::poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			warn("timed out waiting for sensor data");
+			warn("ERROR: TIMEOUT 1");
 			goto out;
 		}
 
@@ -1102,7 +1100,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		sz = ::read(fd, &report, sizeof(report));
 
 		if (sz != sizeof(report)) {
-			warn("periodic read failed");
+			warn("ERROR: READ 1");
 			ret = -EIO;
 			goto out;
 		}
@@ -1118,7 +1116,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		ret = ::poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			warn("timed out waiting for sensor data");
+			warn("ERROR: TIMEOUT 2");
 			goto out;
 		}
 
@@ -1126,7 +1124,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		sz = ::read(fd, &report, sizeof(report));
 
 		if (sz != sizeof(report)) {
-			warn("periodic read failed");
+			warn("ERROR: READ 2");
 			ret = -EIO;
 			goto out;
 		}
@@ -1142,33 +1140,19 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 			sum_excited[1] += cal[1];
 			sum_excited[2] += cal[2];
 		}
-
-		//warnx("periodic read %u", i);
-		//warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
-		//warnx("cal: %.6f  %.6f  %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
 	}
 
 	if (good_count < 5) {
-		warn("failed calibration");
 		ret = -EIO;
 		goto out;
 	}
 
-#if 0
-	warnx("measurement avg: %.6f  %.6f  %.6f", 
-	      (double)sum_excited[0]/good_count, 
-	      (double)sum_excited[1]/good_count, 
-	      (double)sum_excited[2]/good_count);
-#endif
-
 	float scaling[3];
 
 	scaling[0] = sum_excited[0] / good_count;
 	scaling[1] = sum_excited[1] / good_count;
 	scaling[2] = sum_excited[2] / good_count;
 
-	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
-
 	/* set scaling in device */
 	mscale_previous.x_scale = scaling[0];
 	mscale_previous.y_scale = scaling[1];
@@ -1179,29 +1163,26 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 out:
 
 	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
-		warn("failed to set new scale / offsets for mag");
+		warn("FAILED: MAGIOCSSCALE 2");
 	}
 
 	/* set back to normal mode */
 	/* Set to 1.1 Gauss */
 	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
-		warnx("failed to set 1.1 Ga range");
+		warnx("FAILED: MAGIOCSRANGE 1.1 Ga");
 	}
 
 	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
-		warnx("failed to disable sensor calibration mode");
+		warnx("FAILED: MAGIOCEXSTRAP 0");
 	}
 
 	if (ret == OK) {
-		if (!check_scale()) {
-			warnx("mag scale calibration successfully finished.");
-		} else {
-			warnx("mag scale calibration finished with invalid results.");
+		if (check_scale()) {
+			/* failed */
+			warnx("FAILED: SCALE");
 			ret = ERROR;
 		}
 
-	} else {
-		warnx("mag scale calibration failed.");
 	}
 
 	return ret;

src/drivers/hmc5883/module.mk

@@ -42,3 +42,5 @@ SRCS		= hmc5883.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os

src/drivers/hott/comms.cpp

@@ -55,7 +55,7 @@ open_uart(const char *device)
 	const int uart = open(device, O_RDWR | O_NOCTTY);
 
 	if (uart < 0) {
-		err(1, "Error opening port: %s", device);
+		err(1, "ERR: opening %s", device);
 	}
 	
 	/* Back up the original uart configuration to restore it after exit */	
@@ -63,7 +63,7 @@ open_uart(const char *device)
 	struct termios uart_config_original;
 	if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
 		close(uart);
-		err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
+		err(1, "ERR: %s: %d", device, termios_state);
 	}
 
 	/* Fill the struct for the new configuration */
@@ -76,13 +76,13 @@ open_uart(const char *device)
 	/* Set baud rate */
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
 		close(uart);
-		err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
+		err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)",
 			 device, termios_state);
 	}
 
 	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
 		close(uart);
-		err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
+		err(1, "ERR: %s (tcsetattr)", device);
 	}
 
 	/* Activate single wire mode */

src/drivers/hott/hott_sensors/hott_sensors.cpp

@@ -204,7 +204,7 @@ hott_sensors_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			warnx("deamon already running");
+			warnx("already running");
 			exit(0);
 		}
 
@@ -225,10 +225,10 @@ hott_sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("daemon is running");
+			warnx("is running");
 
 		} else {
-			warnx("daemon not started");
+			warnx("not started");
 		}
 
 		exit(0);

src/drivers/hott/hott_telemetry/hott_telemetry.cpp

@@ -230,7 +230,7 @@ hott_telemetry_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			warnx("deamon already running");
+			warnx("already running");
 			exit(0);
 		}
 
@@ -251,10 +251,10 @@ hott_telemetry_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("daemon is running");
+			warnx("is running");
 
 		} else {
-			warnx("daemon not started");
+			warnx("not started");
 		}
 
 		exit(0);

src/drivers/l3gd20/module.mk

@@ -8,3 +8,5 @@ SRCS		 = l3gd20.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os

src/drivers/lsm303d/module.mk

@@ -8,3 +8,5 @@ SRCS		 = lsm303d.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os

src/drivers/meas_airspeed/meas_airspeed.cpp

@@ -519,7 +519,7 @@ test()
 		ret = poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			errx(1, "timed out waiting for sensor data");
+			errx(1, "timed out");
 		}
 
 		/* now go get it */

src/drivers/mpu6000/module.mk

@@ -42,3 +42,5 @@ SRCS		= mpu6000.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os

src/drivers/px4flow/px4flow.cpp

@@ -76,7 +76,7 @@
 #define PX4FLOW_BUS 			PX4_I2C_BUS_EXPANSION
 #define I2C_FLOW_ADDRESS 		0x42 //* 7-bit address. 8-bit address is 0x84
 //range 0x42 - 0x49
- 
+
 /* PX4FLOW Registers addresses */
 #define PX4FLOW_REG	0x00		/* Measure Register */
 
@@ -200,7 +200,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
 	_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
 {
 	// enable debug() calls
-	_debug_enabled = true;
+	_debug_enabled = false;
 
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
@@ -240,7 +240,7 @@ PX4FLOW::init()
 	_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
 
 	if (_px4flow_topic < 0) {
-		debug("failed to create px4flow object. Did you start uOrb?");
+		warnx("failed to create px4flow object. Did you start uOrb?");
 	}
 
 	ret = OK;
@@ -442,8 +442,6 @@ PX4FLOW::measure()
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		log("i2c::transfer returned %d", ret);
-		printf("i2c::transfer flow returned %d");
 		return ret;
 	}
 
@@ -465,7 +463,7 @@ PX4FLOW::collect()
 	ret = transfer(nullptr, 0, &val[0], 22);
 
 	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;
@@ -565,7 +563,7 @@ PX4FLOW::cycle()
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
+			debug("collection error");
 			/* restart the measurement state machine */
 			start();
 			return;
@@ -592,7 +590,7 @@ PX4FLOW::cycle()
 
 	/* measurement phase */
 	if (OK != measure()) {
-		log("measure error");
+		debug("measure error");
 	}
 
 	/* next phase is collection */

src/drivers/px4fmu/module.mk

@@ -8,3 +8,5 @@ SRCS		 = fmu.cpp
 MODULE_STACKSIZE = 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os

src/drivers/px4io/module.mk

@@ -48,3 +48,5 @@ INCLUDE_DIRS    += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/com
 MODULE_STACKSIZE = 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os

src/drivers/px4io/px4io.cpp

@@ -1245,34 +1245,40 @@ PX4IO::io_set_rc_config()
 	 *       for compatibility reasons with existing
 	 *       autopilots / GCS'.
 	 */
+
+	/* ROLL */
 	param_get(param_find("RC_MAP_ROLL"), &ichan);
-
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		input_map[ichan - 1] = 0;
+	}
 
+	/* PITCH */
 	param_get(param_find("RC_MAP_PITCH"), &ichan);
-
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		input_map[ichan - 1] = 1;
+	}
 
+	/* YAW */
 	param_get(param_find("RC_MAP_YAW"), &ichan);
-
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		input_map[ichan - 1] = 2;
+	}
 
+	/* THROTTLE */
 	param_get(param_find("RC_MAP_THROTTLE"), &ichan);
-
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		input_map[ichan - 1] = 3;
+	}
 
+	/* FLAPS */
 	param_get(param_find("RC_MAP_FLAPS"), &ichan);
-
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		input_map[ichan - 1] = 4;
+	}
 
+	/* MAIN MODE SWITCH */
 	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
-
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		/* use out of normal bounds index to indicate special channel */
 		input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
 	}
@@ -1651,10 +1657,6 @@ PX4IO::io_publish_raw_rc()
 int
 PX4IO::io_publish_pwm_outputs()
 {
-	/* if no FMU comms(!) just don't publish */
-	if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
-		return OK;
-
 	/* data we are going to fetch */
 	actuator_outputs_s outputs;
 	outputs.timestamp = hrt_absolute_time();
@@ -2055,7 +2057,7 @@ PX4IO::print_status(bool extended_status)
 		((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
 		);
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
-	printf("arming 0x%04x%s%s%s%s%s%s%s%s\n",
+	printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
 	       arming,
 	       ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)		? " FMU_ARMED" : " FMU_DISARMED"),
 	       ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)		? " IO_ARM_OK" : " IO_ARM_DENIED"),
@@ -2065,7 +2067,8 @@ PX4IO::print_status(bool extended_status)
 	       ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)	? " ALWAYS_PWM_ENABLE" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)		? " LOCKDOWN" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)		? " FORCE_FAILSAFE" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "")
+	       ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "")
 	       );
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
@@ -2305,6 +2308,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		}
 		break;
 
+	case PWM_SERVO_SET_OVERRIDE_IMMEDIATE:
+		/* control whether override on FMU failure is
+                   immediate or waits for override threshold on mode
+                   switch */
+		if (arg == 0) {
+			/* clear override immediate flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0);
+		} else {
+			/* set override immediate flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE);
+		}
+		break;
+
 	case DSM_BIND_START:
 
 		/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
@@ -2566,6 +2582,42 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 
 		break;
 
+	case PWM_SERVO_SET_RC_CONFIG: {
+		/* enable setting of RC configuration without relying
+		   on param_get() 
+		*/
+		struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
+		if (config->channel >= _max_actuators) {
+			/* fail with error */
+			return E2BIG;
+		}
+
+		/* copy values to registers in IO */
+		uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE];
+		uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE;
+		regs[PX4IO_P_RC_CONFIG_MIN]        = config->rc_min;
+		regs[PX4IO_P_RC_CONFIG_CENTER]     = config->rc_trim;
+		regs[PX4IO_P_RC_CONFIG_MAX]        = config->rc_max;
+		regs[PX4IO_P_RC_CONFIG_DEADZONE]   = config->rc_dz;
+		regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment;
+		regs[PX4IO_P_RC_CONFIG_OPTIONS]    = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+		if (config->rc_reverse) {
+			regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE;
+		}
+		ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
+		break;
+	}
+
+	case PWM_SERVO_SET_OVERRIDE_OK:
+		/* set the 'OVERRIDE OK' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK);
+		break;
+
+	case PWM_SERVO_CLEAR_OVERRIDE_OK:
+		/* clear the 'OVERRIDE OK' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
+		break;
+
 	default:
 		/* see if the parent class can make any use of it */
 		ret = CDev::ioctl(filep, cmd, arg);

src/drivers/stm32/adc/module.mk

@@ -40,3 +40,5 @@ MODULE_COMMAND	= adc
 SRCS		= adc.cpp
 
 INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os

src/drivers/stm32/module.mk

@@ -41,3 +41,5 @@ SRCS		= drv_hrt.c \
 		  drv_pwm_servo.c
 
 INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os

src/drivers/stm32/tone_alarm/module.mk

@@ -40,3 +40,5 @@ MODULE_COMMAND	= tone_alarm
 SRCS		= tone_alarm.cpp
 
 INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os

src/lib/external_lgpl/tecs/tecs.cpp

@@ -321,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 			ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
 		}
 
-		// Calculate PD + FF throttle
+		// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
 		_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+		_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
 
 		// Rate limit PD + FF throttle
 		// Calculate the throttle increment from the specified slew time
 		if (fabsf(_throttle_slewrate) > 0.01f) {
 			float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
-
 			_throttle_dem = constrain(_throttle_dem,
-						  _last_throttle_dem - thrRateIncr,
-						  _last_throttle_dem + thrRateIncr);
+						_last_throttle_dem - thrRateIncr,
+						_last_throttle_dem + thrRateIncr);
 		}
 
 		// Ensure _last_throttle_dem is always initialized properly
-		// Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
 		_last_throttle_dem = _throttle_dem;
 
-
 		// Calculate integrator state upper and lower limits
-		// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
+		// Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
 		float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
 		float integ_min = (_THRminf - _throttle_dem - 0.1f);
 
 		// Calculate integrator state, constraining state
-		// Set integrator to a max throttle value dduring climbout
+		// Set integrator to a max throttle value during climbout
 		_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
 
 		if (_climbOutDem) {

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -206,10 +206,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			      0,  0,  1.f
 			     };		/**< init: identity matrix */
 
-	// print text
-	printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
-	fflush(stdout);
-
 	int overloadcounter = 19;
 
 	/* Initialize filter */

src/modules/bottle_drop/bottle_drop.cpp

@@ -223,7 +223,7 @@ BottleDrop::start()
 	_main_task = task_spawn_cmd("bottle_drop",
 				    SCHED_DEFAULT,
 				    SCHED_PRIORITY_DEFAULT + 15,
-				    2048,
+				    1500,
 				    (main_t)&BottleDrop::task_main_trampoline,
 				    nullptr);
 
@@ -283,7 +283,6 @@ BottleDrop::drop()
 	// force the door open if we have to
 	if (_doors_opened == 0) {
 		open_bay();
-		warnx("bay not ready, forced open");
 	}
 
 	while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) {
@@ -723,16 +722,16 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
 		if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) {
 			open_bay();
 			drop();
-			mavlink_log_info(_mavlink_fd, "#audio: drop bottle");
+			mavlink_log_critical(_mavlink_fd, "drop bottle");
 
 		} else if (cmd->param1 > 0.5f) {
 			open_bay();
-			mavlink_log_info(_mavlink_fd, "#audio: opening bay");
+			mavlink_log_critical(_mavlink_fd, "opening bay");
 
 		} else {
 			lock_release();
 			close_bay();
-			mavlink_log_info(_mavlink_fd, "#audio: closing bay");
+			mavlink_log_critical(_mavlink_fd, "closing bay");
 		}
 
 		answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
@@ -743,12 +742,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
 		switch ((int)(cmd->param1 + 0.5f)) {
 		case 0:
 			_drop_approval = false;
-			mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval");
+			mavlink_log_critical(_mavlink_fd, "got drop position, no approval");
 			break;
 
 		case 1:
 			_drop_approval = true;
-			mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval");
+			mavlink_log_critical(_mavlink_fd, "got drop position and approval");
 			break;
 
 		default:
@@ -818,19 +817,19 @@ BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESUL
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command);
 		break;
 
 	case VEHICLE_CMD_RESULT_FAILED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command);
 		break;
 
 	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command);
 		break;
 
 	case VEHICLE_CMD_RESULT_UNSUPPORTED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command);
 		break;
 
 	default:

src/modules/bottle_drop/module.mk

@@ -39,3 +39,7 @@ MODULE_COMMAND		= bottle_drop
 
 SRCS			= bottle_drop.cpp \
 			  bottle_drop_params.c
+
+MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 1200

src/modules/commander/commander.cpp

@@ -220,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
-void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
-
 transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
 
 void set_control_mode();
@@ -312,12 +310,16 @@ int commander_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "arm")) {
-		arm_disarm(true, mavlink_fd, "command line");
+		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+		arm_disarm(true, mavlink_fd_local, "command line");
+		close(mavlink_fd_local);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "disarm")) {
-		arm_disarm(false, mavlink_fd, "command line");
+		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+		arm_disarm(false, mavlink_fd_local, "command line");
+		close(mavlink_fd_local);
 		exit(0);
 	}
 
@@ -760,7 +762,10 @@ int commander_thread_main(int argc, char *argv[])
 	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
 	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
 	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER]		= "AUTO_RCRECOVER";
 	nav_states_str[NAVIGATION_STATE_AUTO_RTGS]		= "AUTO_RTGS";
+	nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL]	= "AUTO_LANDENGFAIL";
+	nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL]	= "AUTO_LANDGPSFAIL";
 	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
 	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
 	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
@@ -1848,7 +1853,11 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (status.failsafe != failsafe_old) {
 			status_changed = true;
-			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
+			if (status.failsafe) {
+				mavlink_log_critical(mavlink_fd, "failsafe mode on");
+			} else {
+				mavlink_log_critical(mavlink_fd, "failsafe mode off");
+			}
 			failsafe_old = status.failsafe;
 		}
 
@@ -2196,6 +2205,59 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+	case NAVIGATION_STATE_ALTCTL:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
+	case NAVIGATION_STATE_POSCTL:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = true;
+		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
+	case NAVIGATION_STATE_AUTO_MISSION:
+	case NAVIGATION_STATE_AUTO_LOITER:
+	case NAVIGATION_STATE_AUTO_RTL:
+	case NAVIGATION_STATE_AUTO_RCRECOVER:
+	case NAVIGATION_STATE_AUTO_RTGS:
+	case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = true;
+		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
+	case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
 	case NAVIGATION_STATE_ACRO:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
@@ -2208,18 +2270,46 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_ALTCTL:
-		control_mode.flag_control_manual_enabled = true;
-		control_mode.flag_control_auto_enabled = false;
+
+	case NAVIGATION_STATE_LAND:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
 		control_mode.flag_control_attitude_enabled = true;
+		/* in failsafe LAND mode position may be not available */
+		control_mode.flag_control_position_enabled = status.condition_local_position_valid;
+		control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
 		control_mode.flag_control_altitude_enabled = true;
 		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
+	case NAVIGATION_STATE_DESCEND:
+		/* TODO: check if this makes sense */
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
 		control_mode.flag_control_position_enabled = false;
 		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = true;
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+	case NAVIGATION_STATE_TERMINATION:
+		/* disable all controllers on termination */
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = false;
+		control_mode.flag_control_attitude_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_termination_enabled = true;
+		break;
+
 	case NAVIGATION_STATE_OFFBOARD:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = false;
@@ -2275,73 +2365,6 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 		}
-
-		break;
-
-	case NAVIGATION_STATE_POSCTL:
-		control_mode.flag_control_manual_enabled = true;
-		control_mode.flag_control_auto_enabled = false;
-		control_mode.flag_control_rates_enabled = true;
-		control_mode.flag_control_attitude_enabled = true;
-		control_mode.flag_control_altitude_enabled = true;
-		control_mode.flag_control_climb_rate_enabled = true;
-		control_mode.flag_control_position_enabled = true;
-		control_mode.flag_control_velocity_enabled = true;
-		control_mode.flag_control_termination_enabled = false;
-		break;
-
-	case NAVIGATION_STATE_AUTO_MISSION:
-	case NAVIGATION_STATE_AUTO_LOITER:
-	case NAVIGATION_STATE_AUTO_RTL:
-	case NAVIGATION_STATE_AUTO_RTGS:
-	case NAVIGATION_STATE_AUTO_LANDENGFAIL:
-		control_mode.flag_control_manual_enabled = false;
-		control_mode.flag_control_auto_enabled = true;
-		control_mode.flag_control_rates_enabled = true;
-		control_mode.flag_control_attitude_enabled = true;
-		control_mode.flag_control_altitude_enabled = true;
-		control_mode.flag_control_climb_rate_enabled = true;
-		control_mode.flag_control_position_enabled = true;
-		control_mode.flag_control_velocity_enabled = true;
-		control_mode.flag_control_termination_enabled = false;
-		break;
-
-	case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
-		control_mode.flag_control_manual_enabled = false;
-		control_mode.flag_control_auto_enabled = false;
-		control_mode.flag_control_rates_enabled = true;
-		control_mode.flag_control_attitude_enabled = true;
-		control_mode.flag_control_altitude_enabled = false;
-		control_mode.flag_control_climb_rate_enabled = true;
-		control_mode.flag_control_position_enabled = false;
-		control_mode.flag_control_velocity_enabled = false;
-		control_mode.flag_control_termination_enabled = false;
-		break;
-
-	case NAVIGATION_STATE_LAND:
-		control_mode.flag_control_manual_enabled = false;
-		control_mode.flag_control_auto_enabled = true;
-		control_mode.flag_control_rates_enabled = true;
-		control_mode.flag_control_attitude_enabled = true;
-		/* in failsafe LAND mode position may be not available */
-		control_mode.flag_control_position_enabled = status.condition_local_position_valid;
-		control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
-		control_mode.flag_control_altitude_enabled = true;
-		control_mode.flag_control_climb_rate_enabled = true;
-		control_mode.flag_control_termination_enabled = false;
-		break;
-
-	case NAVIGATION_STATE_TERMINATION:
-		/* disable all controllers on termination */
-		control_mode.flag_control_manual_enabled = false;
-		control_mode.flag_control_auto_enabled = false;
-		control_mode.flag_control_rates_enabled = false;
-		control_mode.flag_control_attitude_enabled = false;
-		control_mode.flag_control_position_enabled = false;
-		control_mode.flag_control_velocity_enabled = false;
-		control_mode.flag_control_altitude_enabled = false;
-		control_mode.flag_control_climb_rate_enabled = false;
-		control_mode.flag_control_termination_enabled = true;
 		break;
 
 	default:

src/modules/commander/state_machine_helper.cpp

@@ -444,7 +444,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
  * Check failsafe and main status and set navigation status for navigator accordingly
  */
 bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
-		const bool stay_in_failsafe)
+		   const bool stay_in_failsafe)
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
@@ -497,11 +497,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		break;
 
 	case MAIN_STATE_AUTO_MISSION:
+
 		/* go into failsafe
 		 * - if commanded to do so
 		 * - if we have an engine failure
-		 * - if either the datalink is enabled and lost as well as RC is lost
-		 * - if there is no datalink and the mission is finished */
+		 * - depending on datalink, RC and if the mission is finished */
+
+		/* first look at the commands */
 		if (status->engine_failure_cmd) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		} else if (status->data_link_lost_cmd) {
@@ -509,14 +511,17 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		} else if (status->gps_failure_cmd) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
 		} else if (status->rc_signal_lost_cmd) {
-			status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX
-		/* Finished handling commands which have priority , now handle failures */
+			status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+
+		/* finished handling commands which have priority, now handle failures */
 		} else if (status->gps_failure) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
 		} else if (status->engine_failure) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
-		} else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
-		    (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
+
+		/* datalink loss enabled:
+		 * check for datalink lost: this should always trigger RTGS */
+		} else if (data_link_loss_enabled && status->data_link_lost) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -529,12 +534,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* also go into failsafe if just datalink is lost */
-		} else if (status->data_link_lost && data_link_loss_enabled) {
+		/* datalink loss disabled:
+		 * check if both, RC and datalink are lost during the mission
+		 * or RC is lost after the mission is finished: this should always trigger RCRECOVER */
+		} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
+						       (status->rc_signal_lost && mission_finished))) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+				status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
 			} else if (status->condition_local_position_valid) {
 				status->nav_state = NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
@@ -543,13 +551,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* don't bother if RC is lost and mission is not yet finished */
-		} else if (status->rc_signal_lost && !stay_in_failsafe) {
-
-			/* this mode is ok, we don't need RC for missions */
-			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+		/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
 		} else if (!stay_in_failsafe){
-			/* everything is perfect */
 			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
 		}
 		break;

src/modules/controllib/module.mk

@@ -39,3 +39,5 @@ SRCS		 = test_params.c \
 		   block/BlockParam.cpp \
 		   uorb/blocks.cpp \
 		   blocks.cpp
+
+MAXOPTIMIZATION = -Os

src/modules/dataman/dataman.c

@@ -629,9 +629,6 @@ task_main(int argc, char *argv[])
 {
 	work_q_item_t *work;
 
-	/* inform about start */
-	warnx("Initializing..");
-
 	/* Initialize global variables */
 	g_key_offsets[0] = 0;
 
@@ -694,16 +691,15 @@ task_main(int argc, char *argv[])
 		if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
 			warnx("Power on restart");
 			_restart(DM_INIT_REASON_POWER_ON);
-		}
-		else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
+		} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
 			warnx("In flight restart");
 			_restart(DM_INIT_REASON_IN_FLIGHT);
-		}
-		else
+		} else {
 			warnx("Unknown restart");
-	}
-	else
+		}
+	} else {
 		warnx("Unknown restart");
+	}
 
 	/* We use two file descriptors, one for the caller context and one for the worker thread */
 	/* They are actually the same but we need to some way to reject caller request while the */

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -613,8 +613,11 @@ FixedwingEstimator::check_filter_state()
 			warn_index = max_warn_index;
 		}
 
-		warnx("reset: %s", feedback[warn_index]);
-		mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
+		// Do not warn about accel offset if we have no position updates
+		if (!(warn_index == 5 && _ekf->staticMode)) {
+			warnx("reset: %s", feedback[warn_index]);
+			mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
+		}
 	}
 
 	struct estimator_status_report rep;
@@ -1557,7 +1560,7 @@ FixedwingEstimator::start()
 	_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 40,
-					 5000,
+					 7500,
 					 (main_t)&FixedwingEstimator::task_main_trampoline,
 					 nullptr);
 

src/modules/mavlink/mavlink.c

@@ -78,11 +78,7 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
 
 mavlink_system_t mavlink_system = {
 	100,
-	50,
-	MAV_TYPE_FIXED_WING,
-	0,
-	0,
-	0
+	50
 }; // System ID, 1-255, Component/Subsystem ID, 1-255
 
 /*

src/modules/mavlink/mavlink_main.cpp

@@ -167,8 +167,10 @@ Mavlink::Mavlink() :
 	_param_initialized(false),
 	_param_system_id(0),
 	_param_component_id(0),
-	_param_system_type(0),
+	_param_system_type(MAV_TYPE_FIXED_WING),
 	_param_use_hil_gps(0),
+	_param_forward_externalsp(0),
+	_system_type(0),
 
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
@@ -524,7 +526,7 @@ void Mavlink::mavlink_update_system(void)
 	param_get(_param_system_type, &system_type);
 
 	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
-		mavlink_system.type = system_type;
+		_system_type = system_type;
 	}
 
 	int32_t use_hil_gps;
@@ -755,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
 
 	pthread_mutex_lock(&_send_mutex);
 
-	int buf_free = get_free_tx_buf();
+	unsigned buf_free = get_free_tx_buf();
 
 	uint8_t payload_len = mavlink_message_lengths[msgid];
 	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -820,14 +822,14 @@ Mavlink::resend_message(mavlink_message_t *msg)
 
 	pthread_mutex_lock(&_send_mutex);
 
-	int buf_free = get_free_tx_buf();
+	unsigned buf_free = get_free_tx_buf();
 
 	_last_write_try_time = hrt_absolute_time();
 
 	unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 
 	/* check if there is space in the buffer, let it overflow else */
-	if (buf_free < TX_BUFFER_GAP) {
+	if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) {
 		/* no enough space in buffer to send */
 		count_txerr();
 		count_txerrbytes(packet_len);
@@ -1634,7 +1636,7 @@ Mavlink::start(int argc, char *argv[])
 	task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
-		       2900,
+		       2800,
 		       (main_t)&Mavlink::start_helper,
 		       (const char **)argv);
 

src/modules/mavlink/mavlink_main.h

@@ -265,6 +265,8 @@ public:
 
 	struct mavlink_logbuffer	*get_logbuffer() { return &_logbuffer; }
 
+	unsigned		get_system_type() { return _system_type; }
+
 protected:
 	Mavlink			*next;
 
@@ -354,6 +356,8 @@ private:
 	param_t			_param_use_hil_gps;
 	param_t			_param_forward_externalsp;
 
+	unsigned		_system_type;
+
 	perf_counter_t		_loop_perf;			/**< loop performance counter */
 	perf_counter_t		_txerr_perf;			/**< TX error counter */
 

src/modules/mavlink/mavlink_messages.cpp

@@ -302,7 +302,7 @@ protected:
 		msg.base_mode = 0;
 		msg.custom_mode = 0;
 		get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
-		msg.type = mavlink_system.type;
+		msg.type = _mavlink->get_system_type();
 		msg.autopilot = MAV_AUTOPILOT_PX4;
 		msg.mavlink_version = 3;
 
@@ -382,11 +382,11 @@ protected:
 						clock_gettime(CLOCK_REALTIME, &ts);
 						/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
 						time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
-						struct tm t;
-						gmtime_r(&gps_time_sec, &t);
+						struct tm tt;
+						gmtime_r(&gps_time_sec, &tt);
 
 						// XXX we do not want to interfere here with the SD log app
-						strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t);
+						strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
 						snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
 						fp = fopen(log_file_path, "ab");
 					}
@@ -1353,15 +1353,17 @@ protected:
 
 			const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
 
+			unsigned system_type = _mavlink->get_system_type();
+
 			/* scale outputs depending on system type */
-			if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
-				mavlink_system.type == MAV_TYPE_HEXAROTOR ||
-				mavlink_system.type == MAV_TYPE_OCTOROTOR) {
+			if (system_type == MAV_TYPE_QUADROTOR ||
+				system_type == MAV_TYPE_HEXAROTOR ||
+				system_type == MAV_TYPE_OCTOROTOR) {
 				/* multirotors: set number of rotor outputs depending on type */
 
 				unsigned n;
 
-				switch (mavlink_system.type) {
+				switch (system_type) {
 				case MAV_TYPE_QUADROTOR:
 					n = 4;
 					break;
@@ -1717,7 +1719,53 @@ protected:
 			msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX;
 			msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX;
 			msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX;
-			msg.rssi = rc.rssi;
+
+			/* RSSI has a max value of 100, and when Spektrum or S.BUS are
+			 * available, the RSSI field is invalid, as they do not provide
+			 * an RSSI measurement. Use an out of band magic value to signal
+			 * these digital ports. XXX revise MAVLink spec to address this.
+			 * One option would be to use the top bit to toggle between RSSI
+			 * and input source mode.
+			 *
+			 * Full RSSI field: 0b 1 111 1111
+			 *
+			 *                     ^ If bit is set, RSSI encodes type + RSSI
+			 *
+			 *                       ^ These three bits encode a total of 8
+			 *                         digital RC input types.
+			 *                         0: PPM, 1: SBUS, 2: Spektrum, 2: ST24
+			 *                           ^ These four bits encode a total of
+			 *                             16 RSSI levels. 15 = full, 0 = no signal
+			 *
+			 */
+
+			/* Initialize RSSI with the special mode level flag */
+			msg.rssi = (1 << 7);
+
+			/* Set RSSI */
+			msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15;
+
+			switch (rc.input_source) {
+				case RC_INPUT_SOURCE_PX4FMU_PPM:
+				/* fallthrough */
+				case RC_INPUT_SOURCE_PX4IO_PPM:
+					msg.rssi |= (0 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_SPEKTRUM:
+					msg.rssi |= (1 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_SBUS:
+					msg.rssi |= (2 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_ST24:
+					msg.rssi |= (3 << 4);
+					break;
+			}
+
+			if (rc.rc_lost) {
+				/* RSSI is by definition zero */
+				msg.rssi = 0;
+			}
 
 			_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
 		}

src/modules/mavlink/mavlink_receiver.cpp

@@ -538,12 +538,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 			offboard_control_sp.ignore &=  ~(1 << i);
 			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
 		}
+
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAW);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 10)) <<
-					OFB_IGN_BIT_YAW;
+		if (set_position_target_local_ned.type_mask & (1 << 10)) {
+			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAW);
+		}
+
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAWRATE);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 11)) <<
-					OFB_IGN_BIT_YAWRATE;
+		if (set_position_target_local_ned.type_mask & (1 << 11)) {
+			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAWRATE);
+		}
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -721,8 +721,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
 void
 MulticopterAttitudeControl::task_main()
 {
-	warnx("started");
-	fflush(stdout);
 
 	/*
 	 * do subscriptions

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp()
 				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
 		_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
 				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
-		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
+		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
 	}
 }
 
@@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp()
 	if (_reset_alt_sp) {
 		_reset_alt_sp = false;
 		_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
-		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
+		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
 	}
 }
 
@@ -857,10 +857,8 @@ MulticopterPositionControl::control_auto(float dt)
 void
 MulticopterPositionControl::task_main()
 {
-	warnx("started");
 
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(_mavlink_fd, "[mpc] started");
 
 	/*
 	 * do subscriptions

src/modules/navigator/datalinkloss.cpp

@@ -156,6 +156,7 @@ DataLinkLoss::set_dll_item()
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		warnx("not switched to manual: request flight termination");
 		pos_sp_triplet->previous.valid = false;
 		pos_sp_triplet->current.valid = false;
@@ -188,6 +189,7 @@ DataLinkLoss::advance_dll()
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
 			_navigator->get_mission_result()->stay_in_failsafe = true;
 			_navigator->publish_mission_result();
+			reset_mission_item_reached();
 			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		} else {
 			if (!_param_skipcommshold.get()) {
@@ -208,6 +210,7 @@ DataLinkLoss::advance_dll()
 		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		break;
 	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
 		_dll_state = DLL_STATE_TERMINATE;
@@ -215,6 +218,7 @@ DataLinkLoss::advance_dll()
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		break;
 	case DLL_STATE_TERMINATE:
 		warnx("dll end");

src/modules/navigator/mission.cpp

@@ -371,7 +371,7 @@ Mission::set_mission_items()
 	} else {
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished");
 
 			/* use last setpoint for loiter */
 			_navigator->set_can_loiter_at_sp(true);
@@ -595,13 +595,15 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 	}
 
-	if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
-		/* mission item index out of bounds */
-		return false;
-	}
-
-	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
+	/* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
+	 * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
 	for (int i = 0; i < 10; i++) {
+
+		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
+			/* mission item index out of bounds */
+			return false;
+		}
+
 		const ssize_t len = sizeof(struct mission_item_s);
 
 		/* read mission item to temp storage first to not overwrite current mission item if data damaged */
@@ -626,11 +628,12 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 				if (is_current) {
 					(mission_item_tmp.do_jump_current_count)++;
 					/* save repeat count */
-					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) {
+					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
+					    &mission_item_tmp, len) != len) {
 						/* not supposed to happen unless the datamanager can't access the
 						 * dataman */
 						mavlink_log_critical(_navigator->get_mavlink_fd(),
-								"ERROR DO JUMP waypoint could not be written");
+								     "ERROR DO JUMP waypoint could not be written");
 						return false;
 					}
 				}
@@ -639,8 +642,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
 
 			} else {
-				mavlink_log_critical(_navigator->get_mavlink_fd(),
-						 "DO JUMP repetitions completed");
+				if (is_current) {
+					mavlink_log_critical(_navigator->get_mavlink_fd(),
+							     "DO JUMP repetitions completed");
+				}
 				/* no more DO_JUMPS, therefore just try to continue with next mission item */
 				(*mission_index_ptr)++;
 			}
@@ -707,6 +712,7 @@ Mission::set_mission_item_reached()
 	_navigator->get_mission_result()->reached = true;
 	_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
 	_navigator->publish_mission_result();
+	reset_mission_item_reached();
 }
 
 void