Merged master

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['R0']         = self.mon_reg_call('r0')
-			self.regs['R1']         = long(gdb.parse_and_eval('$r1'))
+			self.regs['R1']         = self.mon_reg_call('r1')
-			self.regs['R2']         = long(gdb.parse_and_eval('$r2'))
+			self.regs['R2']         = self.mon_reg_call('r2')
-			self.regs['R3']         = long(gdb.parse_and_eval('$r3'))
+			self.regs['R3']         = self.mon_reg_call('r3')
-			self.regs['R4']         = long(gdb.parse_and_eval('$r4'))
+			self.regs['R4']         = self.mon_reg_call('r4')
-			self.regs['R5']         = long(gdb.parse_and_eval('$r5'))
+			self.regs['R5']         = self.mon_reg_call('r5')
-			self.regs['R6']         = long(gdb.parse_and_eval('$r6'))
+			self.regs['R6']         = self.mon_reg_call('r6')
-			self.regs['R7']         = long(gdb.parse_and_eval('$r7'))
+			self.regs['R7']         = self.mon_reg_call('r7')
-			self.regs['R8']         = long(gdb.parse_and_eval('$r8'))
+			self.regs['R8']         = self.mon_reg_call('r8')
-			self.regs['R9']         = long(gdb.parse_and_eval('$r9'))
+			self.regs['R9']         = self.mon_reg_call('r9')
-			self.regs['R10']        = long(gdb.parse_and_eval('$r10'))
+			self.regs['R10']        = self.mon_reg_call('r10')
-			self.regs['R11']        = long(gdb.parse_and_eval('$r11'))
+			self.regs['R11']        = self.mon_reg_call('r11')
-			self.regs['R12']        = long(gdb.parse_and_eval('$r12'))
+			self.regs['R12']        = self.mon_reg_call('r12')
-			self.regs['R13']        = long(gdb.parse_and_eval('$r13'))
+			self.regs['R13']        = self.mon_reg_call('r13')
-			self.regs['SP']         = long(gdb.parse_and_eval('$sp'))
+			self.regs['SP']         = self.mon_reg_call('sp')
-			self.regs['R14']        = long(gdb.parse_and_eval('$r14'))
+			self.regs['R14']        = self.mon_reg_call('r14')
-			self.regs['LR']         = long(gdb.parse_and_eval('$lr'))
+			self.regs['LR']         = self.mon_reg_call('lr')
-			self.regs['R15']        = long(gdb.parse_and_eval('$r15'))
+			self.regs['R15']        = self.mon_reg_call('r15')
-			self.regs['PC']         = long(gdb.parse_and_eval('$pc'))
+			self.regs['PC']         = self.mon_reg_call('pc')
-			self.regs['XPSR']       = long(gdb.parse_and_eval('$xpsr'))
+			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()
+			try: 
-			waitsem_holder = waitsem['holder']
+				waitsem = self._tcb['waitsem'].dereference()
-			holder = NX_task.for_tcb(waitsem_holder['htcb'])
+				waitsem_holder = waitsem['holder']
-			if holder is not None:
+				holder = NX_task.for_tcb(waitsem_holder['htcb'])
-				return '{}({})'.format(waitsem.address, holder.name)
+				if holder is not None:
-			else:
+					return '{}({})'.format(waitsem.address, holder.name)
-				return '{}(<bad holder>)'.format(waitsem.address)
+				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')
+		struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s')
-                preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof
+		preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof
-                if preceding_size == 2:
+		if preceding_size == 2:
 			self._allocflag = 0x8000
-                elif preceding_size == 4:
+		elif preceding_size == 4:
 			self._allocflag = 0x80000000
-                else:
+		else:
-                        raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size)
+			raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size)
-                self._allocnodesize = struct_mm_allocnode_s.sizeof
+			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,
+			print( '  {} {} {}'.format(allocnode.address + self._allocnodesize,
-                                                  self._node_size(allocnode), state)
+                                                  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()

LICENSE.md

@@ -0,0 +1,41 @@
+The PX4 firmware is licensed generally under a permissive 3-clause BSD license. Contributions are required
+to be made under the same license. Any exception to this general rule is listed below.
+
+	/****************************************************************************
+	 *
+	 *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+	 *
+	 * Redistribution and use in source and binary forms, with or without
+	 * modification, are permitted provided that the following conditions
+	 * are met:
+	 *
+	 * 1. Redistributions of source code must retain the above copyright
+	 *    notice, this list of conditions and the following disclaimer.
+	 * 2. Redistributions in binary form must reproduce the above copyright
+	 *    notice, this list of conditions and the following disclaimer in
+	 *    the documentation and/or other materials provided with the
+	 *    distribution.
+	 * 3. Neither the name PX4 nor the names of its contributors may be
+	 *    used to endorse or promote products derived from this software
+	 *    without specific prior written permission.
+	 *
+	 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+	 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+	 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+	 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+	 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+	 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+	 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+	 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+	 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+	 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+	 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+	 * POSSIBILITY OF SUCH DAMAGE.
+	 *
+	 ****************************************************************************/
+
+
+  - PX4 middleware: BSD 3-clause
+  - PX4 flight control stack: BSD 3-clause
+  - NuttX operating system: BSD 3-clause
+  - Exceptions: Currently only this [400 LOC file](https://github.com/PX4/Firmware/blob/master/src/lib/external_lgpl/tecs/tecs.cpp) remains LGPL, but will be replaced with a BSD implementation.

NuttX

@@ -1 +1 @@
-Subproject commit 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211c
+Subproject commit 9d06b645790e1445f14e3b19c71d40b3088f4e4f

README.md

@@ -0,0 +1,10 @@
+## PX4 Aerial Middleware and Flight Control Stack ##
+
+*   Official Website: http://px4.io
+*   License: BSD 3-clause (see LICENSE.md)
+*   Supported airframes:
+  * Multicopters
+  * Fixed wing
+*   Binaries (always up-to-date from master):
+  * [Downloads](https://pixhawk.org/downloads)
+*   Mailing list: [Google Groups](http://groups.google.com/group/px4users)

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

@@ -1,14 +1,10 @@
 #!nsh
 #
-# HILStar / X-Plane
+# HILStar
-#
+# <lorenz@px4.io>
-# Lorenz Meier <lm@inf.ethz.ch>
 #
 
 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
@@ -27,3 +27,4 @@ fi
 set MIXER FMU_quad_w
 
 set PWM_OUTPUTS 1234
+set PWM_MIN 1200

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>
+# Thomas Gubler <thomas@px4.io>
-# Lorenz Meier <lm@inf.ethz.ch>
 #
 
 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,11 +2,10 @@
 #
 # Viper
 #
-# Simon Wilks <sjwilks@gmail.com>
+# Simon Wilks <simon@px4.io>
 #
 
 sh /etc/init.d/rc.fw_defaults
 
 set MIXER Viper
 
-set FAILSAFE "-c567 -p 1000"

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,9 +17,15 @@
 # 12000	..	12999		Octo Cox
 
 #
-# Simulation setups
+# Simulation
 #
 
+if param compare SYS_AUTOSTART 901
+then
+	sh /etc/init.d/901_bottle_drop_test.hil
+	set MODE custom
+fi
+
 if param compare SYS_AUTOSTART 1000
 then
 	sh /etc/init.d/1000_rc_fw_easystar.hil
@@ -47,7 +52,7 @@ then
 fi
 
 #
-# Standard plane
+# Plane
 #
 
 if param compare SYS_AUTOSTART 2100 100

ROMFS/px4fmu_common/init.d/rc.fw_apps

@@ -13,3 +13,5 @@ ekf_att_pos_estimator start
 #
 fw_att_control start
 fw_pos_control_l1 start
+
+bottle_drop start

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,10 +54,8 @@ then
 	fi
 fi
 
-# Start airspeed sensors
 if meas_airspeed start
 then
-	echo "[init] Using MEAS airspeed sensor"
 else
 	if ets_airspeed start
 	then
@@ -74,10 +72,7 @@ then
 fi
 
 #
-# Start the sensor collection task.
+# Start sensors -> preflight_check
-# IMPORTANT: this also loads param offsets
-# ALWAYS start this task before the
-# 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
+		# Wipe out params
-		# if someone requests a re-configuration, we do it
-		# cleanly from scratch (except autostart / autoconfig)
 		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
 
-	#
+	# waypoint storage
-	# Start the datamanager (and do not abort boot if it fails)
-	#
 	if dataman start
 	then
 	fi
 
-	#
+	# Needs to be this early for in-air-restarts
-	# Start the Commander (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
@@ -461,10 +447,6 @@ 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 ]

ROMFS/px4fmu_common/mixers/FMU_FX79.mix

@@ -52,21 +52,18 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
-Gimbal / flaps / payload mixer for last four channels
+Inputs to the mixer come from channel group 2 (payload), channels 0
+(bay servo 1), 1 (bay servo 2) and 3 (drop release).
 -----------------------------------------------------
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 4  10000  10000      0 -10000  10000
+S: 2 0  10000  10000      0 -10000  10000
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 5  10000  10000      0 -10000  10000
+S: 2 1  10000  10000      0 -10000  10000
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 6  10000  10000      0 -10000  10000
+S: 2 2 -10000 -10000      0 -10000  10000
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 7  10000  10000      0 -10000  10000

ROMFS/px4fmu_common/mixers/Viper.mix

@@ -66,6 +66,6 @@ S: 2 1  10000  10000      0 -10000  10000
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 2 2 -10000 -10000      0 -10000  10000
+S: 2 2  -8000  -8000      0 -10000  10000
 
 

ROMFS/px4fmu_test/init.d/rcS

@@ -75,3 +75,33 @@ if [ -f /fs/microsd/mount_test_cmds.txt ]
 then
 	tests mount
 fi
+
+#
+# Run unit tests at board boot, reporting failure as needed.
+# Add new unit tests using the same pattern as below.
+#
+
+set unit_test_failure 0
+
+if mavlink_tests
+then
+else
+	set unit_test_failure 1
+	set unit_test_failure_list "${unit_test_failure_list} mavlink_tests"
+fi
+
+if commander_tests
+then
+else
+	set unit_test_failure 1
+	set unit_test_failure_list "${unit_test_failure_list} commander_tests"
+fi
+
+if [ $unit_test_failure == 0 ]
+then
+	echo
+	echo "All Unit Tests PASSED"
+else
+	echo
+	echo "Some Unit Tests FAILED:${unit_test_failure_list}"
+fi

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_romfs_pruner.py

@@ -57,7 +57,7 @@ def main():
         for (root, dirs, files) in os.walk(args.folder):
                 for file in files:
                         # only prune text files
-                        if ".zip" in file or ".bin" in file or ".swp" in file:
+                        if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file:
                                 continue
 
                         file_path = os.path.join(root, file)

Tools/px_uploader.py

@@ -178,9 +178,9 @@ class uploader(object):
         MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0')
         MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac')
 
-        def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5):
+        def __init__(self, portname, baudrate):
                 # open the port, keep the default timeout short so we can poll quickly
-                self.port = serial.Serial(portname, baudrate)
+                self.port = serial.Serial(portname, baudrate, timeout=0.5)
                 self.otp = b''
                 self.sn = b''
 
@@ -195,7 +195,7 @@ class uploader(object):
         def __recv(self, count=1):
                 c = self.port.read(count)
                 if len(c) < 1:
-                        raise RuntimeError("timeout waiting for data (%u bytes)", count)
+                        raise RuntimeError("timeout waiting for data (%u bytes)" % count)
 #               print("recv " + binascii.hexlify(c))
                 return c
 
@@ -458,7 +458,8 @@ 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
 while True:
@@ -508,9 +509,12 @@ while True:
                 except Exception:
                         # most probably a timeout talking to the port, no bootloader, try to reboot the board
                         print("attempting reboot on %s..." % port)
+                        print("if the board does not respond, unplug and re-plug the USB connector.")
                         up.send_reboot()
                         # wait for the reboot, without we might run into Serial I/O Error 5 
                         time.sleep(0.5)
+                        # always close the port
+                        up.close()
                         continue
 
                 try:

Tools/sdlog2/sdlog2_dump.py

@@ -154,8 +154,8 @@ class SDLog2Parser:
                         first_data_msg = False
                     self.__parseMsg(msg_descr)
             bytes_read += self.__ptr
-        if not self.__debug_out and self.__time_msg != None and self.__csv_updated:
+            if not self.__debug_out and self.__time_msg != None and self.__csv_updated:
-            self.__printCSVRow()
+                self.__printCSVRow()
         f.close()
     
     def __bytesLeft(self):

Tools/tests-host/.gitignore

@@ -1,4 +1,6 @@
 ./obj/*
 mixer_test
+sf0x_test
 sbus2_test
 autodeclination_test
+st24_test

Tools/tests-host/Makefile

@@ -3,7 +3,7 @@ CC=g++
 CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \
 	-I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm
 
-all: mixer_test sbus2_test autodeclination_test
+all: mixer_test sbus2_test autodeclination_test st24_test sf0x_test
 
 MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \
 		../../src/systemcmds/tests/test_conv.cpp \
@@ -20,7 +20,16 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \
 		hrt.cpp \
 		sbus2_test.cpp
 
-AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \
+ST24_FILES=../../src/lib/rc/st24.c \
+		hrt.cpp \
+		st24_test.cpp
+
+SF0X_FILES= \
+		hrt.cpp \
+		sf0x_test.cpp \
+		../../src/drivers/sf0x/sf0x_parser.cpp
+
+AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \
 		hrt.cpp \
 		autodeclination_test.cpp
 
@@ -30,10 +39,16 @@ mixer_test: $(MIXER_FILES)
 sbus2_test: $(SBUS2_FILES)
 	$(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS)
 
+sf0x_test: $(SF0X_FILES)
+	$(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS)
+
 autodeclination_test: $(SBUS2_FILES)
 	$(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS)
 
+st24_test: $(ST24_FILES)
+	$(CC) -o st24_test $(ST24_FILES) $(CFLAGS)
+
 .PHONY: clean
 
 clean:
-	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test
+	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test st24_test sf0x_test

Tools/tests-host/sbus2_test.cpp

@@ -29,7 +29,8 @@ int main(int argc, char *argv[]) {
 
 	// Trash the first 20 lines
 	for (unsigned i = 0; i < 20; i++) {
-		(void)fscanf(fp, "%f,%x,,", &f, &x);
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
 	}
 
 	// Init the parser

Tools/tests-host/sf0x_test.cpp

@@ -0,0 +1,65 @@
+
+#include <unistd.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#include <drivers/sf0x/sf0x_parser.h>
+
+int main(int argc, char *argv[])
+{
+	warnx("SF0X test started");
+
+	int ret = 0;
+
+	const char LINE_MAX = 20;
+	char _linebuf[LINE_MAX];
+	_linebuf[0] = '\0';
+
+	const char *lines[] = {"0.01\r\n",
+			       "0.02\r\n",
+			       "0.03\r\n",
+			       "0.04\r\n",
+			       "0",
+			       ".",
+			       "0",
+			       "5",
+			       "\r",
+			       "\n",
+			       "0",
+			       "3\r",
+			       "\n"
+			       "\r\n",
+			       "0.06",
+			       "\r\n"
+			      };
+
+	enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC;
+	float dist_m;
+	char _parserbuf[LINE_MAX];
+	unsigned _parsebuf_index = 0;
+
+	for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) {
+
+		printf("\n%s", _linebuf);
+
+		int parse_ret;
+
+		for (int i = 0; i < strlen(lines[l]); i++) {
+			parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
+
+			if (parse_ret == 0) {
+				printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
+			}
+		}
+
+		printf("%s", lines[l]);
+
+	}
+
+	warnx("test finished");
+
+	return ret;
+}

Tools/tests-host/st24_test.cpp

@@ -0,0 +1,76 @@
+
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <rc/st24.h>
+#include "../../src/systemcmds/tests/tests.h"
+
+int main(int argc, char *argv[])
+{
+	warnx("ST24 test started");
+
+	if (argc < 2) {
+		errx(1, "Need a filename for the input file");
+	}
+
+	warnx("loading data from: %s", argv[1]);
+
+	FILE *fp;
+
+	fp = fopen(argv[1], "rt");
+
+	if (!fp) {
+		errx(1, "failed opening file");
+	}
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
+	}
+
+	float last_time = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+		if (((f - last_time) * 1000 * 1000) > 3000) {
+			// warnx("FRAME RESET\n\n");
+		}
+
+		uint8_t b = static_cast<uint8_t>(x);
+
+		last_time = f;
+
+		// Pipe the data into the parser
+		hrt_abstime now = hrt_absolute_time();
+
+		uint8_t rssi;
+		uint8_t rx_count;
+		uint16_t channel_count;
+		uint16_t channels[20];
+
+
+		if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) {
+			warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
+
+			for (unsigned i = 0; i < channel_count; i++) {
+
+				int16_t val = channels[i];
+				warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
+			}
+		}
+	}
+
+	if (ret == EOF) {
+		warnx("Test finished, reached end of file");
+
+	} else {
+		warnx("Test aborted, errno: %d", ret);
+	}
+
+}

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

Tools/usb_serialload.py

@@ -0,0 +1,55 @@
+import serial, time
+
+
+port = serial.Serial('/dev/ttyACM0', baudrate=57600, timeout=2)
+
+data = '01234567890123456789012345678901234567890123456789'
+#data = 'hellohello'
+outLine = 'echo %s\n' % data
+
+port.write('\n\n\n')
+port.write('free\n')
+line = port.readline(80)
+while line != '':
+    print(line)
+    line = port.readline(80)
+    
+
+i = 0
+bytesOut = 0
+bytesIn = 0
+
+startTime = time.time()
+lastPrint = startTime
+while True:
+    bytesOut += port.write(outLine)
+    line = port.readline(80)
+    bytesIn += len(line)
+    # check command line echo
+    if (data not in line):
+        print('command error %d: %s' % (i,line))
+        #break
+    # read echo output
+    line = port.readline(80)
+    if (data not in line):
+        print('echo output error %d: %s' % (i,line))
+        #break
+    bytesIn += len(line)
+    #print('%d: %s' % (i,line))
+    #print('%d: bytesOut: %d, bytesIn: %d' % (i, bytesOut, bytesIn))
+    
+    elapsedT = time.time() - lastPrint
+    if (time.time() - lastPrint >= 5):
+        outRate = bytesOut / elapsedT
+        inRate = bytesIn / elapsedT
+        usbRate = (bytesOut + bytesIn) / elapsedT
+        lastPrint = time.time()
+        print('elapsed time: %f' % (time.time() - startTime))
+        print('data rates (bytes/sec): out: %f, in: %f, total: %f' % (outRate, inRate, usbRate))
+        
+        bytesOut = 0
+        bytesIn = 0
+        
+    i += 1    
+    #if (i > 2): break
+    

makefiles/config_px4fmu-v1_default.mk

@@ -24,25 +24,22 @@ MODULES		+= drivers/l3gd20
 MODULES		+= drivers/mpu6000
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
-MODULES		+= drivers/mb12xx
+#MODULES		+= drivers/mb12xx
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
-MODULES		+= drivers/hott/hott_telemetry
+#MODULES		+= drivers/blinkm
-MODULES		+= drivers/hott/hott_sensors
-MODULES		+= drivers/blinkm
 MODULES		+= drivers/rgbled
 MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/airspeed
-MODULES		+= drivers/ets_airspeed
+#MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
-MODULES		+= drivers/frsky_telemetry
+#MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors
 
 #
 # System commands
 #
 MODULES		+= systemcmds/mtd
-MODULES		+= systemcmds/bl_update
 MODULES		+= systemcmds/mixer
 MODULES		+= systemcmds/param
 MODULES		+= systemcmds/perf
@@ -135,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
 #
@@ -128,13 +113,18 @@ MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
 
+#
+# OBC challenge
+#
+MODULES		+= modules/bottle_drop
+
 #
 # Demo apps
 #
 #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
@@ -151,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

@@ -49,11 +49,22 @@ MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/conversion
 
+#
+# Modules to test-build, but not useful for test environment
+#
+MODULES		+= modules/attitude_estimator_so3
+MODULES		+= drivers/pca8574
+MODULES		+= examples/flow_position_estimator
+
 #
 # Libraries
 #
 LIBRARIES	+= lib/mathlib/CMSIS
 
+MODULES 	+= modules/unit_test
+MODULES		+= modules/mavlink/mavlink_tests
+MODULES 	+= modules/commander/commander_tests
+
 #
 # Transitional support - add commands from the NuttX export archive.
 #

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)))
+ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED)))
-$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED).x)
+$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED))
 endif
 
 

mavlink/include/mavlink/v1.0

@@ -1 +1 @@
-Subproject commit 2423e47b4f9169e77f7194b36fa2118e018c94e2
+Subproject commit ad5e5a645dec152419264ad32221f7c113ea5c30

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-v1/nsh/defconfig

@@ -287,7 +287,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y
 #
 # CONFIG_STM32_I2C_DYNTIMEO is not set
 CONFIG_STM32_I2CTIMEOSEC=0
-CONFIG_STM32_I2CTIMEOMS=1
+CONFIG_STM32_I2CTIMEOMS=10
 # CONFIG_STM32_I2C_DUTY16_9 is not set
 
 #

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);
+		warnx("%s\n", reason);
-	fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d <UART>]\n\n");
+	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/aerocore_init.c

@@ -93,6 +93,19 @@
 #  endif
 #endif
 
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
 /****************************************************************************
  * Protected Functions
  ****************************************************************************/

src/drivers/boards/aerocore/board_config.h

@@ -171,6 +171,25 @@ __BEGIN_DECLS
 
 extern void stm32_spiinitialize(void);
 
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization for NSH.
+ *
+ *   CONFIG_NSH_ARCHINIT=y :
+ *     Called from the NSH library
+ *
+ *   CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
+ *   CONFIG_NSH_ARCHINIT=n :
+ *     Called from board_initialize().
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_NSH_LIBRARY
+int nsh_archinitialize(void);
+#endif
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS

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/board_config.h

@@ -209,6 +209,27 @@ __BEGIN_DECLS
 
 extern void stm32_spiinitialize(void);
 
+extern void stm32_usbinitialize(void);
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization for NSH.
+ *
+ *   CONFIG_NSH_ARCHINIT=y :
+ *     Called from the NSH library
+ *
+ *   CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
+ *   CONFIG_NSH_ARCHINIT=n :
+ *     Called from board_initialize().
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_NSH_LIBRARY
+int nsh_archinitialize(void);
+#endif
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS

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/board_config.h

@@ -229,6 +229,27 @@ __BEGIN_DECLS
 
 extern void stm32_spiinitialize(void);
 
+extern void stm32_usbinitialize(void);
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization for NSH.
+ *
+ *   CONFIG_NSH_ARCHINIT=y :
+ *     Called from the NSH library
+ *
+ *   CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
+ *   CONFIG_NSH_ARCHINIT=n :
+ *     Called from board_initialize().
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_NSH_LIBRARY
+int nsh_archinitialize(void);
+#endif
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS

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/px4fmu-v2/px4fmu2_init.c

@@ -94,6 +94,19 @@
 #  endif
 #endif
 
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
 /****************************************************************************
  * Protected Functions
  ****************************************************************************/

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/board_config.h

@@ -77,6 +77,7 @@
 #define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
 #define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
 #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
 
 /* Safety switch button *******************************************************/
 

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

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

src/drivers/boards/px4io-v2/px4iov2_init.c

@@ -108,6 +108,7 @@ __EXPORT void stm32_boardinitialize(void)
 	stm32_configgpio(GPIO_LED1);
 	stm32_configgpio(GPIO_LED2);
 	stm32_configgpio(GPIO_LED3);
+	stm32_configgpio(GPIO_LED4);
 
 	stm32_configgpio(GPIO_BTN_SAFETY);
 

src/drivers/device/device.cpp

@@ -97,6 +97,7 @@ Device::Device(const char *name,
         
 	/* setup a default device ID. When bus_type is UNKNOWN the
 	   other fields are invalid */
+	_device_id.devid = 0;
 	_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
 	_device_id.devid_s.bus = 0;
 	_device_id.devid_s.address = 0;

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_gps.h

@@ -57,7 +57,8 @@
 typedef enum {
 	GPS_DRIVER_MODE_NONE = 0,
 	GPS_DRIVER_MODE_UBX,
-	GPS_DRIVER_MODE_MTK
+	GPS_DRIVER_MODE_MTK,
+	GPS_DRIVER_MODE_ASHTECH
 } gps_driver_mode_t;
 
 

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.
  */
@@ -165,7 +182,7 @@ ORB_DECLARE(output_pwm);
 
 #define DSM2_BIND_PULSES 3	/* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
 #define DSMX_BIND_PULSES 7	/* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
-#define DSMX8_BIND_PULSES 10 	/* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
+#define DSMX8_BIND_PULSES 9 	/* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
 
 /** power up DSM receiver */
 #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
@@ -205,10 +222,28 @@ ORB_DECLARE(output_pwm);
 #define PWM_SERVO_GET_DISABLE_LOCKDOWN		_IOC(_PWM_SERVO_BASE, 22)
 
 /** force safety switch off (to disable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
+#define PWM_SERVO_SET_FORCE_SAFETY_OFF		_IOC(_PWM_SERVO_BASE, 23)
 
 /** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
-#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
+#define PWM_SERVO_SET_FORCE_FAILSAFE		_IOC(_PWM_SERVO_BASE, 24)
+
+/** make failsafe non-recoverable (termination) if it occurs */
+#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)
+
+/** 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/drv_rc_input.h

@@ -83,7 +83,8 @@ enum RC_INPUT_SOURCE {
 	RC_INPUT_SOURCE_PX4FMU_PPM,
 	RC_INPUT_SOURCE_PX4IO_PPM,
 	RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
-	RC_INPUT_SOURCE_PX4IO_SBUS
+	RC_INPUT_SOURCE_PX4IO_SBUS,
+	RC_INPUT_SOURCE_PX4IO_ST24
 };
 
 /**

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/ashtech.cpp

@@ -0,0 +1,641 @@
+#include "ashtech.h"
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <string.h>
+#include <assert.h>
+#include <systemlib/err.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
+#include <drivers/drv_hrt.h>
+
+#include <fcntl.h>
+#include <math.h>
+
+ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info):
+	_fd(fd),
+	_satellite_info(satellite_info),
+	_gps_position(gps_position)
+{
+	decode_init();
+	_decode_state = NME_DECODE_UNINIT;
+	_rx_buffer_bytes = 0;
+}
+
+ASHTECH::~ASHTECH()
+{
+}
+
+/*
+ * All NMEA descriptions are taken from
+ * http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html
+ */
+
+int ASHTECH::handle_message(int len)
+{
+	char * endp;
+	
+	if (len < 7) { return 0; }
+
+	int uiCalcComma = 0;
+
+	for (int i = 0 ; i < len; i++) {
+		if (_rx_buffer[i] == ',') { uiCalcComma++; }
+	}
+
+	char *bufptr = (char *)(_rx_buffer + 6);
+
+	if ((memcmp(_rx_buffer + 3, "ZDA,", 3) == 0) && (uiCalcComma == 6)) {
+		/*
+		UTC day, month, and year, and local time zone offset
+		An example of the ZDA message string is:
+
+		$GPZDA,172809.456,12,07,1996,00,00*45
+
+		ZDA message fields
+		Field	Meaning
+		0	Message ID $GPZDA
+		1	UTC
+		2	Day, ranging between 01 and 31
+		3	Month, ranging between 01 and 12
+		4	Year
+		5	Local time zone offset from GMT, ranging from 00 through 13 hours
+		6	Local time zone offset from GMT, ranging from 00 through 59 minutes
+		7	The checksum data, always begins with *
+		Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT.
+		*/
+		double ashtech_time = 0.0;
+		int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 0;
+
+		if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+
+		int ashtech_hour = ashtech_time / 10000;
+		int ashtech_minute = (ashtech_time - ashtech_hour * 10000) / 100;
+		double ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100;
+		/*
+		 * convert to unix timestamp
+		 */
+		struct tm timeinfo;
+		timeinfo.tm_year = year - 1900;
+		timeinfo.tm_mon = month - 1;
+		timeinfo.tm_mday = day;
+		timeinfo.tm_hour = ashtech_hour;
+		timeinfo.tm_min = ashtech_minute;
+		timeinfo.tm_sec = int(ashtech_sec);
+		time_t epoch = mktime(&timeinfo);
+
+		_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+		_gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6);
+		_gps_position->timestamp_time = hrt_absolute_time();
+	}
+
+	else if ((memcmp(_rx_buffer + 3, "GGA,", 3) == 0) && (uiCalcComma == 14)) {
+		/*
+		  Time, position, and fix related data
+		  An example of the GBS message string is:
+
+		  $GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F
+
+		  Note - The data string exceeds the ASHTECH standard length.
+		  GGA message fields
+		  Field   Meaning
+		  0   Message ID $GPGGA
+		  1   UTC of position fix
+		  2   Latitude
+		  3   Direction of latitude:
+		  N: North
+		  S: South
+		  4   Longitude
+		  5   Direction of longitude:
+		  E: East
+		  W: West
+		  6   GPS Quality indicator:
+		  0: Fix not valid
+		  1: GPS fix
+		  2: Differential GPS fix, OmniSTAR VBS
+		  4: Real-Time Kinematic, fixed integers
+		  5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK
+		  7   Number of SVs in use, range from 00 through to 24+
+		  8   HDOP
+		  9   Orthometric height (MSL reference)
+		  10  M: unit of measure for orthometric height is meters
+		  11  Geoid separation
+		  12  M: geoid separation measured in meters
+		  13  Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used.
+		  14  Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1.
+		  15
+		  The checksum data, always begins with *
+		  Note - If a user-defined geoid model, or an inclined
+		*/
+		double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
+		int num_of_sv __attribute__((unused)) = 0, fix_quality = 0;
+		double hdop __attribute__((unused)) = 99.9;
+		char ns = '?', ew = '?';
+
+		if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
+
+		if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
+
+		if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (ns == 'S') {
+			lat = -lat;
+		}
+
+		if (ew == 'W') {
+			lon = -lon;
+		}
+
+		_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
+		_gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
+		_gps_position->alt = alt * 1000;
+		_rate_count_lat_lon++;
+
+		if (fix_quality <= 0) {
+			_gps_position->fix_type = 0;
+
+		} else {
+			/*
+			 * in this NMEA message float integers (value 5) mode has higher value than fixed integers (value 4), whereas it provides lower quality,
+			 * and since value 3 is not being used, I "moved" value 5 to 3 to add it to _gps_position->fix_type
+			 */
+			if (fix_quality == 5) { fix_quality = 3; }
+
+			/*
+			 * fix quality 1 means just a normal 3D fix, so I'm subtracting 1 here. This way we'll have 3 for auto, 4 for DGPS, 5 for floats, 6 for fixed.
+			 */
+			_gps_position->fix_type = 3 + fix_quality - 1;
+		}
+
+		_gps_position->timestamp_position = hrt_absolute_time();
+
+		_gps_position->vel_m_s = 0;                                  /**< GPS ground speed (m/s) */
+		_gps_position->vel_n_m_s = 0;                                /**< GPS ground speed in m/s */
+		_gps_position->vel_e_m_s = 0;                                /**< GPS ground speed in m/s */
+		_gps_position->vel_d_m_s = 0;                                /**< GPS ground speed in m/s */
+		_gps_position->cog_rad =
+			0;                                  /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
+		_gps_position->vel_ned_valid = true;                         /**< Flag to indicate if NED speed is valid */
+		_gps_position->c_variance_rad = 0.1;
+		_gps_position->timestamp_velocity = hrt_absolute_time();
+		return 1;
+
+	} else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) {
+		/*
+		Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34
+
+		    $PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc
+		    Parameter Description Range
+		      d1 Position mode 0: standalone
+		                       1: differential
+		                       2: RTK float
+		                       3: RTK fixed
+		                       5: Dead reckoning
+		                       9: SBAS (see NPT setting)
+		      d2 Number of satellite used in position fix 0-99
+		      m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99
+		      m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes
+		      c5 Latitude sector N, S
+		      m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes
+		      c7 Longitude sector E,W
+		      f8 Altitude above ellipsoid +9999.000
+		      f9 Differential age (data link age), seconds 0.0-600.0
+		      f10 True track/course over ground in degrees 0.0-359.9
+		      f11 Speed over ground in knots 0.0-999.9
+		      f12 Vertical velocity in decimeters per second +999.9
+		      f13 PDOP 0-99.9
+		      f14 HDOP 0-99.9
+		      f15 VDOP 0-99.9
+		      f16 TDOP 0-99.9
+		      s17 Reserved no data
+		      *cc Checksum
+		    */
+		bufptr = (char *)(_rx_buffer + 10);
+
+		/*
+		 * Ashtech would return empty space as coordinate (lat, lon or alt) if it doesn't have a fix yet
+		 */
+		int coordinatesFound = 0;
+		double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
+		int num_of_sv __attribute__((unused)) = 0, fix_quality = 0;
+		double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0;
+		double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9,  pdop __attribute__((unused)) = 99.9, tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0;
+		char ns = '?', ew = '?';
+
+		if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') {
+			/*
+			 * if a coordinate is skipped (i.e. no fix), it either won't get into this block (two commas in a row)
+			 * or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt.
+			 */
+			lat = strtod(bufptr, &endp);
+			if (bufptr != endp) {coordinatesFound++;}
+			bufptr = endp;
+		}
+
+		if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
+
+		if (bufptr && *(++bufptr) != ',') {
+			lon = strtod(bufptr, &endp);
+			if (bufptr != endp) {coordinatesFound++;}
+			bufptr = endp;
+		}
+
+		if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
+
+		if (bufptr && *(++bufptr) != ',') {
+			alt = strtod(bufptr, &endp);
+			if (bufptr != endp) {coordinatesFound++;}
+			bufptr = endp;
+		}
+
+		if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { track_true = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { ground_speed = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { vertic_vel = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { pdop = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { vdop = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { tdop = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (ns == 'S') {
+			lat = -lat;
+		}
+
+		if (ew == 'W') {
+			lon = -lon;
+		}
+
+		_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
+		_gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
+		_gps_position->alt = alt * 1000;
+		_rate_count_lat_lon++;
+
+		if (coordinatesFound < 3) {
+			_gps_position->fix_type = 0;
+
+		} else {
+			_gps_position->fix_type = 3 + fix_quality;
+		}
+
+		_gps_position->timestamp_position = hrt_absolute_time();
+
+		double track_rad = track_true * M_PI / 180.0;
+
+		double velocity_ms = ground_speed  / 1.9438445;			/** knots to m/s */
+		double velocity_north = velocity_ms * cos(track_rad);
+		double velocity_east  = velocity_ms * sin(track_rad);
+
+		_gps_position->vel_m_s = velocity_ms;                            /**< GPS ground speed (m/s) */
+		_gps_position->vel_n_m_s = velocity_north;                       /**< GPS ground speed in m/s */
+		_gps_position->vel_e_m_s = velocity_east;                        /**< GPS ground speed in m/s */
+		_gps_position->vel_d_m_s = -vertic_vel;                      /**< GPS ground speed in m/s */
+		_gps_position->cog_rad =
+			track_rad;                              /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
+		_gps_position->vel_ned_valid = true;                             /**< Flag to indicate if NED speed is valid */
+		_gps_position->c_variance_rad = 0.1;
+		_gps_position->timestamp_velocity = hrt_absolute_time();
+		return 1;
+
+	} else if ((memcmp(_rx_buffer + 3, "GST,", 3) == 0) && (uiCalcComma == 8)) {
+		/*
+		  Position error statistics
+		  An example of the GST message string is:
+
+		  $GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A
+
+		  The Talker ID ($--) will vary depending on the satellite system used for the position solution:
+
+		  $GP - GPS only
+		  $GL - GLONASS only
+		  $GN - Combined
+		  GST message fields
+		  Field   Meaning
+		  0   Message ID $GPGST
+		  1   UTC of position fix
+		  2   RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing
+		  3   Error ellipse semi-major axis 1 sigma error, in meters
+		  4   Error ellipse semi-minor axis 1 sigma error, in meters
+		  5   Error ellipse orientation, degrees from true north
+		  6   Latitude 1 sigma error, in meters
+		  7   Longitude 1 sigma error, in meters
+		  8   Height 1 sigma error, in meters
+		  9   The checksum data, always begins with *
+		*/
+		double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
+		double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0;
+
+		if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { rms_err = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { maj_err = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { min_err = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { deg_from_north = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { lat_err = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { lon_err = strtod(bufptr, &endp); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; }
+
+		_gps_position->eph = sqrt(lat_err * lat_err + lon_err * lon_err);
+		_gps_position->epv = alt_err;
+
+		_gps_position->s_variance_m_s = 0;
+		_gps_position->timestamp_variance = hrt_absolute_time();
+
+	} else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) {
+		/*
+		  The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is:
+
+		  $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67
+
+		  GSV message fields
+		  Field   Meaning
+		  0   Message ID $GPGSV
+		  1   Total number of messages of this type in this cycle
+		  2   Message number
+		  3   Total number of SVs visible
+		  4   SV PRN number
+		  5   Elevation, in degrees, 90 maximum
+		  6   Azimuth, degrees from True North, 000 through 359
+		  7   SNR, 00 through 99 dB (null when not tracking)
+		  8-11    Information about second SV, same format as fields 4 through 7
+		  12-15   Information about third SV, same format as fields 4 through 7
+		  16-19   Information about fourth SV, same format as fields 4 through 7
+		  20  The checksum data, always begins with *
+		*/
+		/*
+		 * currently process only gps, because do not know what
+		 * Global satellite ID I should use for non GPS sats
+		 */
+		bool bGPS = false;
+
+		if (memcmp(_rx_buffer, "$GP", 3) != 0) {
+			return 0;
+
+		} else {
+			bGPS = true;
+		}
+
+		int all_msg_num = 0, this_msg_num = 0, tot_sv_visible = 0;
+		struct gsv_sat {
+			int svid;
+			int elevation;
+			int azimuth;
+			int snr;
+		} sat[4];
+		memset(sat, 0, sizeof(sat));
+
+		if (bufptr && *(++bufptr) != ',') { all_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { this_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+		if (bufptr && *(++bufptr) != ',') { tot_sv_visible = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+		if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) {
+			return 0;
+		}
+
+		if ((this_msg_num == 0) && (bGPS == true)) {
+			memset(_satellite_info->svid,     0, sizeof(_satellite_info->svid));
+			memset(_satellite_info->used,     0, sizeof(_satellite_info->used));
+			memset(_satellite_info->snr,      0, sizeof(_satellite_info->snr));
+			memset(_satellite_info->elevation, 0, sizeof(_satellite_info->elevation));
+			memset(_satellite_info->azimuth,  0, sizeof(_satellite_info->azimuth));
+		}
+
+		int end = 4;
+
+		if (this_msg_num == all_msg_num) {
+			end =  tot_sv_visible - (this_msg_num - 1) * 4;
+			_gps_position->satellites_used = tot_sv_visible;
+			_satellite_info->count = SAT_INFO_MAX_SATELLITES;
+			_satellite_info->timestamp = hrt_absolute_time();
+		}
+
+		for (int y = 0 ; y < end ; y++) {
+			if (bufptr && *(++bufptr) != ',') { sat[y].svid = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+			if (bufptr && *(++bufptr) != ',') { sat[y].elevation = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+			if (bufptr && *(++bufptr) != ',') { sat[y].azimuth = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+			if (bufptr && *(++bufptr) != ',') { sat[y].snr = strtol(bufptr, &endp, 10); bufptr = endp; }
+
+			_satellite_info->svid[y + (this_msg_num - 1) * 4]      = sat[y].svid;
+			_satellite_info->used[y + (this_msg_num - 1) * 4]      = ((sat[y].snr > 0) ? true : false);
+			_satellite_info->snr[y + (this_msg_num - 1) * 4]       = sat[y].snr;
+			_satellite_info->elevation[y + (this_msg_num - 1) * 4] = sat[y].elevation;
+			_satellite_info->azimuth[y + (this_msg_num - 1) * 4]   = sat[y].azimuth;
+		}
+	}
+
+	return 0;
+}
+
+
+int ASHTECH::receive(unsigned timeout)
+{
+	{
+		/* poll descriptor */
+		pollfd fds[1];
+		fds[0].fd = _fd;
+		fds[0].events = POLLIN;
+
+		uint8_t buf[32];
+
+		/* timeout additional to poll */
+		uint64_t time_started = hrt_absolute_time();
+
+		int j = 0;
+		ssize_t bytes_count = 0;
+
+		while (true) {
+
+			/* pass received bytes to the packet decoder */
+			while (j < bytes_count) {
+				int l = 0;
+
+				if ((l = parse_char(buf[j])) > 0) {
+					/* return to configure during configuration or to the gps driver during normal work
+					 * if a packet has arrived */
+					if (handle_message(l) > 0) {
+						return 1;
+					}
+				}
+
+				/* in case we keep trying but only get crap from GPS */
+				if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) {
+					return -1;
+				}
+
+				j++;
+			}
+
+			/* everything is read */
+			j = bytes_count = 0;
+
+			/* then poll for new data */
+			int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout * 2);
+
+			if (ret < 0) {
+				/* something went wrong when polling */
+				return -1;
+
+			} else if (ret == 0) {
+				/* Timeout */
+				return -1;
+
+			} else if (ret > 0) {
+				/* if we have new data from GPS, go handle it */
+				if (fds[0].revents & POLLIN) {
+					/*
+					 * We are here because poll says there is some data, so this
+					 * won't block even on a blocking device.  If more bytes are
+					 * available, we'll go back to poll() again...
+					 */
+					bytes_count = ::read(_fd, buf, sizeof(buf));
+				}
+			}
+		}
+	}
+
+}
+#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA)))
+
+int ASHTECH::parse_char(uint8_t b)
+{
+	int iRet = 0;
+
+	switch (_decode_state) {
+		/* First, look for sync1 */
+	case NME_DECODE_UNINIT:
+		if (b == '$') {
+			_decode_state = NME_DECODE_GOT_SYNC1;
+			_rx_buffer_bytes = 0;
+			_rx_buffer[_rx_buffer_bytes++] = b;
+		}
+
+		break;
+
+	case NME_DECODE_GOT_SYNC1:
+		if (b == '$') {
+			_decode_state = NME_DECODE_GOT_SYNC1;
+			_rx_buffer_bytes = 0;
+
+		} else if (b == '*') {
+			_decode_state = NME_DECODE_GOT_ASTERIKS;
+		}
+
+		if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) {
+			_decode_state = NME_DECODE_UNINIT;
+			_rx_buffer_bytes = 0;
+
+		} else {
+			_rx_buffer[_rx_buffer_bytes++] = b;
+		}
+
+		break;
+
+	case NME_DECODE_GOT_ASTERIKS:
+		_rx_buffer[_rx_buffer_bytes++] = b;
+		_decode_state = NME_DECODE_GOT_FIRST_CS_BYTE;
+		break;
+
+	case NME_DECODE_GOT_FIRST_CS_BYTE:
+		_rx_buffer[_rx_buffer_bytes++] = b;
+		uint8_t checksum = 0;
+		uint8_t *buffer = _rx_buffer + 1;
+		uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3;
+
+		for (; buffer < bufend; buffer++) { checksum ^= *buffer; }
+
+		if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) &&
+		    (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) {
+			iRet = _rx_buffer_bytes;
+		}
+
+		_decode_state = NME_DECODE_UNINIT;
+		_rx_buffer_bytes = 0;
+		break;
+	}
+
+	return iRet;
+}
+
+void ASHTECH::decode_init(void)
+{
+
+}
+
+/* 
+ * ashtech board configuration script 
+ */
+
+const char comm[] = "$PASHS,POP,20\r\n"\
+	      "$PASHS,NME,ZDA,B,ON,3\r\n"\
+	      "$PASHS,NME,GGA,B,OFF\r\n"\
+	      "$PASHS,NME,GST,B,ON,3\r\n"\
+	      "$PASHS,NME,POS,B,ON,0.05\r\n"\
+	      "$PASHS,NME,GSV,B,ON,3\r\n"\
+	      "$PASHS,SPD,A,8\r\n"\
+	      "$PASHS,SPD,B,9\r\n";
+
+int ASHTECH::configure(unsigned &baudrate)
+{
+	/* try different baudrates */
+	const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
+
+
+	for (unsigned int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {
+		baudrate = baudrates_to_try[baud_i];
+		set_baudrate(_fd, baudrate);
+		write(_fd, (uint8_t *)comm, sizeof(comm));
+	}
+
+	set_baudrate(_fd, 115200);
+	return 0;
+}

src/drivers/gps/ashtech.h

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013. All rights reserved.
+ *   Author: Boriskin Aleksey <a.d.boriskin@gmail.com>
+ *           Kistanov Alexander <akistanov@gramant.ru>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* @file ASHTECH protocol definitions */
+
+#ifndef ASHTECH_H_
+#define ASHTECH_H_
+
+#include "gps_helper.h"
+
+#ifndef RECV_BUFFER_SIZE
+#define RECV_BUFFER_SIZE 512
+
+#define SAT_INFO_MAX_SATELLITES  20
+#endif
+
+
+class ASHTECH : public GPS_Helper
+{
+	enum ashtech_decode_state_t {
+		NME_DECODE_UNINIT,
+		NME_DECODE_GOT_SYNC1,
+		NME_DECODE_GOT_ASTERIKS,
+		NME_DECODE_GOT_FIRST_CS_BYTE
+	};
+
+	int                    _fd;
+	struct satellite_info_s *_satellite_info;
+	struct vehicle_gps_position_s *_gps_position;
+	int ashtechlog_fd;
+
+	ashtech_decode_state_t   _decode_state;
+	uint8_t               _rx_buffer[RECV_BUFFER_SIZE];
+	uint16_t              _rx_buffer_bytes;
+	bool                  _parse_error; 		/** parse error flag */
+	char                 *_parse_pos; 		/** parse position */
+
+	bool	_gsv_in_progress;			/**< Indicates that gsv data parsing is in progress */
+	/* int     _satellites_count; 			**< Number of satellites info parsed. */
+	uint8_t count;					/**< Number of satellites in satellite info */
+	uint8_t svid[SAT_INFO_MAX_SATELLITES]; 		/**< Space vehicle ID [1..255], see scheme below  */
+	uint8_t used[SAT_INFO_MAX_SATELLITES];		/**< 0: Satellite not used, 1: used for navigation */
+	uint8_t elevation[SAT_INFO_MAX_SATELLITES];	/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
+	uint8_t azimuth[SAT_INFO_MAX_SATELLITES];	/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
+	uint8_t snr[SAT_INFO_MAX_SATELLITES];		/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
+
+public:
+	ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
+	~ASHTECH();
+	int             receive(unsigned timeout);
+	int             configure(unsigned &baudrate);
+	void            decode_init(void);
+	int             handle_message(int len);
+	int             parse_char(uint8_t b);
+	/** Read int ASHTECH parameter */
+	int32_t         read_int();
+	/** Read float ASHTECH parameter */
+	double       read_float();
+	/** Read char ASHTECH parameter */
+	char            read_char();
+
+};
+
+#endif /* ASHTECH_H_ */

src/drivers/gps/gps.cpp

@@ -69,6 +69,7 @@
 
 #include "ubx.h"
 #include "mtk.h"
+#include "ashtech.h"
 
 
 #define TIMEOUT_5HZ 500
@@ -341,6 +342,10 @@ GPS::task_main()
 				_Helper = new MTK(_serial_fd, &_report_gps_pos);
 				break;
 
+			case GPS_DRIVER_MODE_ASHTECH:
+				_Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info);
+				break;
+
 			default:
 				break;
 			}
@@ -402,6 +407,10 @@ GPS::task_main()
 							mode_str = "MTK";
 							break;
 
+						case GPS_DRIVER_MODE_ASHTECH:
+							mode_str = "ASHTECH";
+							break;
+
 						default:
 							break;
 						}
@@ -429,6 +438,10 @@ GPS::task_main()
 				break;
 
 			case GPS_DRIVER_MODE_MTK:
+				_mode = GPS_DRIVER_MODE_ASHTECH;
+				break;
+
+			case GPS_DRIVER_MODE_ASHTECH:
 				_mode = GPS_DRIVER_MODE_UBX;
 				break;
 
@@ -475,6 +488,10 @@ GPS::print_info()
 		warnx("protocol: MTK");
 		break;
 
+	case GPS_DRIVER_MODE_ASHTECH:
+		warnx("protocol: ASHTECH");
+		break;
+
 	default:
 		break;
 	}

src/drivers/gps/module.mk

@@ -40,6 +40,7 @@ MODULE_COMMAND	= gps
 SRCS		= gps.cpp \
 		  gps_helper.cpp \
 		  mtk.cpp \
+		  ashtech.cpp \
 		  ubx.cpp
 
 MODULE_STACKSIZE = 1200

src/drivers/gps/ubx.cpp

@@ -189,6 +189,18 @@ UBX::configure(unsigned &baudrate)
 		return 1;
 	}
 
+#ifdef UBX_CONFIGURE_SBAS
+	/* send a SBAS message to set the SBAS options */
+	memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas));
+	_buf.payload_tx_cfg_sbas.mode		= UBX_TX_CFG_SBAS_MODE;
+
+	send_message(UBX_MSG_CFG_SBAS, _buf.raw, sizeof(_buf.payload_tx_cfg_sbas));
+
+	if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) {
+		return 1;
+	}
+#endif
+
 	/* configure message rates */
 	/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
 

src/drivers/gps/ubx.h

@@ -73,6 +73,7 @@
 #define UBX_ID_CFG_MSG		0x01
 #define UBX_ID_CFG_RATE		0x08
 #define UBX_ID_CFG_NAV5		0x24
+#define UBX_ID_CFG_SBAS		0x16
 #define UBX_ID_MON_VER		0x04
 #define UBX_ID_MON_HW		0x09
 
@@ -89,6 +90,7 @@
 #define UBX_MSG_CFG_MSG		((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8)
 #define UBX_MSG_CFG_RATE	((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8)
 #define UBX_MSG_CFG_NAV5	((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8)
+#define UBX_MSG_CFG_SBAS	((UBX_CLASS_CFG) | UBX_ID_CFG_SBAS << 8)
 #define UBX_MSG_MON_HW		((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
 #define UBX_MSG_MON_VER		((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
 
@@ -128,6 +130,11 @@
 #define UBX_TX_CFG_NAV5_DYNMODEL	7		/**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
 #define UBX_TX_CFG_NAV5_FIXMODE		2		/**< 1 2D only, 2 3D only, 3 Auto 2D/3D */
 
+/* TX CFG-SBAS message contents */
+#define UBX_TX_CFG_SBAS_MODE_ENABLED	1				/**< SBAS enabled */
+#define UBX_TX_CFG_SBAS_MODE_DISABLED	0				/**< SBAS disabled */
+#define UBX_TX_CFG_SBAS_MODE		UBX_TX_CFG_SBAS_MODE_DISABLED	/**< SBAS enabled or disabled */
+
 /* TX CFG-MSG message contents */
 #define UBX_TX_CFG_MSG_RATE1_5HZ	0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
 #define UBX_TX_CFG_MSG_RATE1_1HZ	0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
@@ -383,6 +390,15 @@ typedef struct {
 	uint32_t	reserved4;
 } ubx_payload_tx_cfg_nav5_t;
 
+/* tx cfg-sbas */
+typedef struct {
+	uint8_t		mode;
+	uint8_t		usage;
+	uint8_t		maxSBAS;
+	uint8_t		scanmode2;
+	uint32_t	scanmode1;
+} ubx_payload_tx_cfg_sbas_t;
+
 /* Tx CFG-MSG */
 typedef struct {
 	union {
@@ -413,6 +429,7 @@ typedef union {
 	ubx_payload_tx_cfg_prt_t		payload_tx_cfg_prt;
 	ubx_payload_tx_cfg_rate_t		payload_tx_cfg_rate;
 	ubx_payload_tx_cfg_nav5_t		payload_tx_cfg_nav5;
+	ubx_payload_tx_cfg_sbas_t		payload_tx_cfg_sbas;
 	ubx_payload_tx_cfg_msg_t		payload_tx_cfg_msg;
 	uint8_t					raw[];
 } ubx_buf_t;

src/drivers/hil/hil.cpp

@@ -392,7 +392,8 @@ HIL::task_main()
 		if (fds[0].revents & POLLIN) {
 
 			/* get controls - must always do this to avoid spinning */
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+			orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1), _t_actuators, &_controls);
 
 			/* can we mix? */
 			if (_mixers != nullptr) {
@@ -441,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()) {
+		if (check_scale()) {
-			warnx("mag scale calibration successfully finished.");
+			/* failed */
-		} else {
+			warnx("FAILED: SCALE");
-			warnx("mag scale calibration finished with invalid results.");
 			ret = ERROR;
 		}
 
-	} else {
-		warnx("mag scale calibration failed.");
 	}
 
 	return ret;