forked from Archive/PX4-Autopilot
Merged master
This commit is contained in:
Lorenz Meier
commit
f3a224e30d
38
Debug/NuttX
38
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
|
||||
433
Debug/Nuttx.py
433
Debug/Nuttx.py
|
|
@ -59,30 +59,42 @@ class NX_register_set(object):
|
|||
|
||||
def __init__(self, xcpt_regs):
|
||||
if xcpt_regs is None:
|
||||
self.regs['R0'] = long(gdb.parse_and_eval('$r0'))
|
||||
self.regs['R1'] = long(gdb.parse_and_eval('$r1'))
|
||||
self.regs['R2'] = long(gdb.parse_and_eval('$r2'))
|
||||
self.regs['R3'] = long(gdb.parse_and_eval('$r3'))
|
||||
self.regs['R4'] = long(gdb.parse_and_eval('$r4'))
|
||||
self.regs['R5'] = long(gdb.parse_and_eval('$r5'))
|
||||
self.regs['R6'] = long(gdb.parse_and_eval('$r6'))
|
||||
self.regs['R7'] = long(gdb.parse_and_eval('$r7'))
|
||||
self.regs['R8'] = long(gdb.parse_and_eval('$r8'))
|
||||
self.regs['R9'] = long(gdb.parse_and_eval('$r9'))
|
||||
self.regs['R10'] = long(gdb.parse_and_eval('$r10'))
|
||||
self.regs['R11'] = long(gdb.parse_and_eval('$r11'))
|
||||
self.regs['R12'] = long(gdb.parse_and_eval('$r12'))
|
||||
self.regs['R13'] = long(gdb.parse_and_eval('$r13'))
|
||||
self.regs['SP'] = long(gdb.parse_and_eval('$sp'))
|
||||
self.regs['R14'] = long(gdb.parse_and_eval('$r14'))
|
||||
self.regs['LR'] = long(gdb.parse_and_eval('$lr'))
|
||||
self.regs['R15'] = long(gdb.parse_and_eval('$r15'))
|
||||
self.regs['PC'] = long(gdb.parse_and_eval('$pc'))
|
||||
self.regs['XPSR'] = long(gdb.parse_and_eval('$xpsr'))
|
||||
self.regs['R0'] = self.mon_reg_call('r0')
|
||||
self.regs['R1'] = self.mon_reg_call('r1')
|
||||
self.regs['R2'] = self.mon_reg_call('r2')
|
||||
self.regs['R3'] = self.mon_reg_call('r3')
|
||||
self.regs['R4'] = self.mon_reg_call('r4')
|
||||
self.regs['R5'] = self.mon_reg_call('r5')
|
||||
self.regs['R6'] = self.mon_reg_call('r6')
|
||||
self.regs['R7'] = self.mon_reg_call('r7')
|
||||
self.regs['R8'] = self.mon_reg_call('r8')
|
||||
self.regs['R9'] = self.mon_reg_call('r9')
|
||||
self.regs['R10'] = self.mon_reg_call('r10')
|
||||
self.regs['R11'] = self.mon_reg_call('r11')
|
||||
self.regs['R12'] = self.mon_reg_call('r12')
|
||||
self.regs['R13'] = self.mon_reg_call('r13')
|
||||
self.regs['SP'] = self.mon_reg_call('sp')
|
||||
self.regs['R14'] = self.mon_reg_call('r14')
|
||||
self.regs['LR'] = self.mon_reg_call('lr')
|
||||
self.regs['R15'] = self.mon_reg_call('r15')
|
||||
self.regs['PC'] = self.mon_reg_call('pc')
|
||||
self.regs['XPSR'] = self.mon_reg_call('xPSR')
|
||||
else:
|
||||
for key in self.v7em_regmap.keys():
|
||||
self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]])
|
||||
|
||||
def mon_reg_call(self,register):
|
||||
"""
|
||||
register is the register as a string e.g. 'pc'
|
||||
return integer containing the value of the register
|
||||
"""
|
||||
str_to_eval = "mon reg "+register
|
||||
resp = gdb.execute(str_to_eval,to_string = True)
|
||||
content = resp.split()[-1];
|
||||
try:
|
||||
return int(content,16)
|
||||
except:
|
||||
return 0
|
||||
|
||||
@classmethod
|
||||
def with_xcpt_regs(cls, xcpt_regs):
|
||||
|
|
@ -172,7 +184,7 @@ class NX_task(object):
|
|||
self.__dict__['stack_used'] = 0
|
||||
else:
|
||||
stack_limit = self._tcb['adj_stack_size']
|
||||
for offset in range(0, stack_limit):
|
||||
for offset in range(0, int(stack_limit)):
|
||||
if stack_base[offset] != 0xff:
|
||||
break
|
||||
self.__dict__['stack_used'] = stack_limit - offset
|
||||
|
|
@ -187,7 +199,7 @@ class NX_task(object):
|
|||
def state(self):
|
||||
"""return the name of the task's current state"""
|
||||
statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e'))
|
||||
for name,value in statenames.iteritems():
|
||||
for name,value in statenames.items():
|
||||
if value == self._tcb['task_state']:
|
||||
return name
|
||||
return 'UNKNOWN'
|
||||
|
|
@ -196,16 +208,19 @@ class NX_task(object):
|
|||
def waiting_for(self):
|
||||
"""return a description of what the task is waiting for, if it is waiting"""
|
||||
if self._state_is('TSTATE_WAIT_SEM'):
|
||||
waitsem = self._tcb['waitsem'].dereference()
|
||||
waitsem_holder = waitsem['holder']
|
||||
holder = NX_task.for_tcb(waitsem_holder['htcb'])
|
||||
if holder is not None:
|
||||
return '{}({})'.format(waitsem.address, holder.name)
|
||||
else:
|
||||
return '{}(<bad holder>)'.format(waitsem.address)
|
||||
try:
|
||||
waitsem = self._tcb['waitsem'].dereference()
|
||||
waitsem_holder = waitsem['holder']
|
||||
holder = NX_task.for_tcb(waitsem_holder['htcb'])
|
||||
if holder is not None:
|
||||
return '{}({})'.format(waitsem.address, holder.name)
|
||||
else:
|
||||
return '{}(<bad holder>)'.format(waitsem.address)
|
||||
except:
|
||||
return 'EXCEPTION'
|
||||
if self._state_is('TSTATE_WAIT_SIG'):
|
||||
return 'signal'
|
||||
return None
|
||||
return ""
|
||||
|
||||
@property
|
||||
def is_waiting(self):
|
||||
|
|
@ -229,7 +244,7 @@ class NX_task(object):
|
|||
filearray = filelist['fl_files']
|
||||
result = dict()
|
||||
for i in range(filearray.type.range()[0],filearray.type.range()[1]):
|
||||
inode = long(filearray[i]['f_inode'])
|
||||
inode = int(filearray[i]['f_inode'])
|
||||
if inode != 0:
|
||||
result[i] = inode
|
||||
return result
|
||||
|
|
@ -253,7 +268,18 @@ class NX_task(object):
|
|||
|
||||
def __str__(self):
|
||||
return "{}:{}".format(self.pid, self.name)
|
||||
|
||||
|
||||
def showoff(self):
|
||||
print("-------")
|
||||
print(self.pid,end = ", ")
|
||||
print(self.name,end = ", ")
|
||||
print(self.state,end = ", ")
|
||||
print(self.waiting_for,end = ", ")
|
||||
print(self.stack_used,end = ", ")
|
||||
print(self._tcb['adj_stack_size'],end = ", ")
|
||||
print(self.file_descriptors)
|
||||
print(self.registers)
|
||||
|
||||
def __format__(self, format_spec):
|
||||
return format_spec.format(
|
||||
pid = self.pid,
|
||||
|
|
@ -265,7 +291,7 @@ class NX_task(object):
|
|||
file_descriptors = self.file_descriptors,
|
||||
registers = self.registers
|
||||
)
|
||||
|
||||
|
||||
class NX_show_task (gdb.Command):
|
||||
"""(NuttX) prints information about a task"""
|
||||
|
||||
|
|
@ -285,7 +311,7 @@ class NX_show_task (gdb.Command):
|
|||
my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n'
|
||||
my_fmt += ' R12 {registers[PC]:#010x}\n'
|
||||
my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n'
|
||||
print format(t, my_fmt)
|
||||
print(format(t, my_fmt))
|
||||
|
||||
class NX_show_tasks (gdb.Command):
|
||||
"""(NuttX) prints a list of tasks"""
|
||||
|
|
@ -295,8 +321,10 @@ class NX_show_tasks (gdb.Command):
|
|||
|
||||
def invoke(self, args, from_tty):
|
||||
tasks = NX_task.tasks()
|
||||
print ('Number of tasks: ' + str(len(tasks)))
|
||||
for t in tasks:
|
||||
print format(t, '{pid:<2} {name:<16} {state:<20} {stack_used:>4}/{stack_limit:<4}')
|
||||
#t.showoff()
|
||||
print(format(t, 'Task: {pid} {name} {state} {stack_used}/{stack_limit}'))
|
||||
|
||||
NX_show_task()
|
||||
NX_show_tasks()
|
||||
|
|
@ -306,15 +334,15 @@ class NX_show_heap (gdb.Command):
|
|||
|
||||
def __init__(self):
|
||||
super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER)
|
||||
struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s')
|
||||
preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof
|
||||
if preceding_size == 2:
|
||||
struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s')
|
||||
preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof
|
||||
if preceding_size == 2:
|
||||
self._allocflag = 0x8000
|
||||
elif preceding_size == 4:
|
||||
elif preceding_size == 4:
|
||||
self._allocflag = 0x80000000
|
||||
else:
|
||||
raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size)
|
||||
self._allocnodesize = struct_mm_allocnode_s.sizeof
|
||||
else:
|
||||
raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size)
|
||||
self._allocnodesize = struct_mm_allocnode_s.sizeof
|
||||
|
||||
def _node_allocated(self, allocnode):
|
||||
if allocnode['preceding'] & self._allocflag:
|
||||
|
|
@ -328,7 +356,7 @@ class NX_show_heap (gdb.Command):
|
|||
if region_start >= region_end:
|
||||
raise gdb.GdbError('heap region {} corrupt'.format(hex(region_start)))
|
||||
nodecount = region_end - region_start
|
||||
print 'heap {} - {}'.format(region_start, region_end)
|
||||
print ('heap {} - {}'.format(region_start, region_end))
|
||||
cursor = 1
|
||||
while cursor < nodecount:
|
||||
allocnode = region_start[cursor]
|
||||
|
|
@ -336,8 +364,8 @@ class NX_show_heap (gdb.Command):
|
|||
state = ''
|
||||
else:
|
||||
state = '(free)'
|
||||
print ' {} {} {}'.format(allocnode.address + self._allocnodesize,
|
||||
self._node_size(allocnode), state)
|
||||
print( ' {} {} {}'.format(allocnode.address + self._allocnodesize,
|
||||
self._node_size(allocnode), state))
|
||||
cursor += self._node_size(allocnode) / self._allocnodesize
|
||||
|
||||
def invoke(self, args, from_tty):
|
||||
|
|
@ -345,7 +373,7 @@ class NX_show_heap (gdb.Command):
|
|||
nregions = heap['mm_nregions']
|
||||
region_starts = heap['mm_heapstart']
|
||||
region_ends = heap['mm_heapend']
|
||||
print '{} heap(s)'.format(nregions)
|
||||
print( '{} heap(s)'.format(nregions))
|
||||
# walk the heaps
|
||||
for i in range(0, nregions):
|
||||
self._print_allocations(region_starts[i], region_ends[i])
|
||||
|
|
@ -370,6 +398,317 @@ class NX_show_interrupted_thread (gdb.Command):
|
|||
my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n'
|
||||
my_fmt += ' R12 {registers[PC]:#010x}\n'
|
||||
my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n'
|
||||
print format(registers, my_fmt)
|
||||
print (format(registers, my_fmt))
|
||||
|
||||
NX_show_interrupted_thread()
|
||||
|
||||
class NX_check_tcb(gdb.Command):
|
||||
""" check the tcb of a task from a address """
|
||||
def __init__(self):
|
||||
super(NX_check_tcb,self).__init__('show tcb', gdb.COMMAND_USER)
|
||||
|
||||
def invoke(self,args,sth):
|
||||
tasks = NX_task.tasks()
|
||||
print("tcb int: ",int(args))
|
||||
print(tasks[int(args)]._tcb)
|
||||
a =tasks[int(args)]._tcb['xcp']['regs']
|
||||
print("relevant registers:")
|
||||
for reg in regmap:
|
||||
hex_addr= hex(int(a[regmap[reg]]))
|
||||
eval_string = 'info line *'+str(hex_addr)
|
||||
print(reg,": ",hex_addr,)
|
||||
NX_check_tcb()
|
||||
|
||||
class NX_tcb(object):
|
||||
def __init__(self):
|
||||
pass
|
||||
|
||||
def is_in(self,arg,list):
|
||||
for i in list:
|
||||
if arg == i:
|
||||
return True;
|
||||
return False
|
||||
|
||||
def find_tcb_list(self,dq_entry_t):
|
||||
tcb_list = []
|
||||
tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer())
|
||||
first_tcb = tcb_ptr.dereference()
|
||||
tcb_list.append(first_tcb);
|
||||
next_tcb = first_tcb['flink'].dereference()
|
||||
while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]):
|
||||
tcb_list.append(next_tcb);
|
||||
old_tcb = next_tcb;
|
||||
next_tcb = old_tcb['flink'].dereference()
|
||||
|
||||
return [t for t in tcb_list if int(t['pid'])<2000]
|
||||
|
||||
def getTCB(self):
|
||||
list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks']
|
||||
tcb_list = [];
|
||||
for l in list_of_listsnames:
|
||||
li = gdb.lookup_global_symbol(l)
|
||||
print(li)
|
||||
cursor = li.value()['head']
|
||||
tcb_list = tcb_list + self.find_tcb_list(cursor)
|
||||
|
||||
class NX_check_stack_order(gdb.Command):
|
||||
""" Check the Stack order corresponding to the tasks """
|
||||
|
||||
def __init__(self):
|
||||
super(NX_check_stack_order,self).__init__('show check_stack', gdb.COMMAND_USER)
|
||||
|
||||
def is_in(self,arg,list):
|
||||
for i in list:
|
||||
if arg == i:
|
||||
return True;
|
||||
return False
|
||||
|
||||
def find_tcb_list(self,dq_entry_t):
|
||||
tcb_list = []
|
||||
tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer())
|
||||
first_tcb = tcb_ptr.dereference()
|
||||
tcb_list.append(first_tcb);
|
||||
next_tcb = first_tcb['flink'].dereference()
|
||||
while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]):
|
||||
tcb_list.append(next_tcb);
|
||||
old_tcb = next_tcb;
|
||||
next_tcb = old_tcb['flink'].dereference()
|
||||
|
||||
return [t for t in tcb_list if int(t['pid'])<2000]
|
||||
|
||||
def getTCB(self):
|
||||
list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks']
|
||||
tcb_list = [];
|
||||
for l in list_of_listsnames:
|
||||
li = gdb.lookup_global_symbol(l)
|
||||
cursor = li.value()['head']
|
||||
tcb_list = tcb_list + self.find_tcb_list(cursor)
|
||||
return tcb_list
|
||||
|
||||
def getSPfromTask(self,tcb):
|
||||
regmap = NX_register_set.v7em_regmap
|
||||
a =tcb['xcp']['regs']
|
||||
return int(a[regmap['SP']])
|
||||
|
||||
def find_closest(self,list,val):
|
||||
tmp_list = [abs(i-val) for i in list]
|
||||
tmp_min = min(tmp_list)
|
||||
idx = tmp_list.index(tmp_min)
|
||||
return idx,list[idx]
|
||||
|
||||
def find_next_stack(self,address,_dict_in):
|
||||
add_list = []
|
||||
name_list = []
|
||||
for key in _dict_in.keys():
|
||||
for i in range(3):
|
||||
if _dict_in[key][i] < address:
|
||||
add_list.append(_dict_in[key][i])
|
||||
if i == 2: # the last one is the processes stack pointer
|
||||
name_list.append(self.check_name(key)+"_SP")
|
||||
else:
|
||||
name_list.append(self.check_name(key))
|
||||
|
||||
idx,new_address = self.find_closest(add_list,address)
|
||||
return new_address,name_list[idx]
|
||||
|
||||
def check_name(self,name):
|
||||
if isinstance(name,(list)):
|
||||
name = name[0];
|
||||
idx = name.find("\\")
|
||||
newname = name[:idx]
|
||||
|
||||
return newname
|
||||
|
||||
def invoke(self,args,sth):
|
||||
tcb = self.getTCB();
|
||||
stackadresses={};
|
||||
for t in tcb:
|
||||
p = [];
|
||||
#print(t.name,t._tcb['stack_alloc_ptr'])
|
||||
p.append(int(t['stack_alloc_ptr']))
|
||||
p.append(int(t['adj_stack_ptr']))
|
||||
p.append(self.getSPfromTask(t))
|
||||
stackadresses[str(t['name'])] = p;
|
||||
address = int("0x30000000",0)
|
||||
print("stack address : process")
|
||||
for i in range(len(stackadresses)*3):
|
||||
address,name = self.find_next_stack(address,stackadresses)
|
||||
print(hex(address),": ",name)
|
||||
|
||||
NX_check_stack_order()
|
||||
|
||||
class NX_run_debug_util(gdb.Command):
|
||||
""" show the registers of a task corresponding to a tcb address"""
|
||||
def __init__(self):
|
||||
super(NX_run_debug_util,self).__init__('show regs', gdb.COMMAND_USER)
|
||||
|
||||
def printRegisters(self,task):
|
||||
regmap = NX_register_set.v7em_regmap
|
||||
a =task._tcb['xcp']['regs']
|
||||
print("relevant registers in ",task.name,":")
|
||||
for reg in regmap:
|
||||
hex_addr= hex(int(a[regmap[reg]]))
|
||||
eval_string = 'info line *'+str(hex_addr)
|
||||
print(reg,": ",hex_addr,)
|
||||
|
||||
def getPCfromTask(self,task):
|
||||
regmap = NX_register_set.v7em_regmap
|
||||
a =task._tcb['xcp']['regs']
|
||||
return hex(int(a[regmap['PC']]))
|
||||
|
||||
def invoke(self,args,sth):
|
||||
tasks = NX_task.tasks()
|
||||
if args == '':
|
||||
for t in tasks:
|
||||
self.printRegisters(t)
|
||||
eval_str = "list *"+str(self.getPCfromTask(t))
|
||||
print("this is the location in code where the current threads $pc is:")
|
||||
gdb.execute(eval_str)
|
||||
else:
|
||||
tcb_nr = int(args);
|
||||
print("tcb_nr = ",tcb_nr)
|
||||
t = tasks[tcb_nr]
|
||||
self.printRegisters(t)
|
||||
eval_str = "list *"+str(self.getPCfromTask(t))
|
||||
print("this is the location in code where the current threads $pc is:")
|
||||
gdb.execute(eval_str)
|
||||
|
||||
NX_run_debug_util()
|
||||
|
||||
|
||||
class NX_search_tcb(gdb.Command):
|
||||
""" shot PID's of all running tasks """
|
||||
|
||||
def __init__(self):
|
||||
super(NX_search_tcb,self).__init__('show alltcb', gdb.COMMAND_USER)
|
||||
|
||||
def is_in(self,arg,list):
|
||||
for i in list:
|
||||
if arg == i:
|
||||
return True;
|
||||
return False
|
||||
|
||||
def find_tcb_list(self,dq_entry_t):
|
||||
tcb_list = []
|
||||
tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer())
|
||||
first_tcb = tcb_ptr.dereference()
|
||||
tcb_list.append(first_tcb);
|
||||
next_tcb = first_tcb['flink'].dereference()
|
||||
while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]):
|
||||
tcb_list.append(next_tcb);
|
||||
old_tcb = next_tcb;
|
||||
next_tcb = old_tcb['flink'].dereference()
|
||||
|
||||
return [t for t in tcb_list if int(t['pid'])<2000]
|
||||
|
||||
def invoke(self,args,sth):
|
||||
list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks']
|
||||
tasks = [];
|
||||
for l in list_of_listsnames:
|
||||
li = gdb.lookup_global_symbol(l)
|
||||
cursor = li.value()['head']
|
||||
tasks = tasks + self.find_tcb_list(cursor)
|
||||
|
||||
# filter for tasks that are listed twice
|
||||
tasks_filt = {}
|
||||
for t in tasks:
|
||||
pid = int(t['pid']);
|
||||
if not pid in tasks_filt.keys():
|
||||
tasks_filt[pid] = t['name'];
|
||||
print('{num_t} Tasks found:'.format(num_t = len(tasks_filt)))
|
||||
for pid in tasks_filt.keys():
|
||||
print("PID: ",pid," ",tasks_filt[pid])
|
||||
|
||||
NX_search_tcb()
|
||||
|
||||
|
||||
class NX_my_bt(gdb.Command):
|
||||
""" 'fake' backtrace: backtrace the stack of a process and check every suspicious address for the list
|
||||
arg: tcb_address$
|
||||
(can easily be found by typing 'showtask').
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
super(NX_my_bt,self).__init__('show mybt', gdb.COMMAND_USER)
|
||||
|
||||
def readmem(self,addr):
|
||||
'''
|
||||
read memory at addr and return nr
|
||||
'''
|
||||
str_to_eval = "x/x "+hex(addr)
|
||||
resp = gdb.execute(str_to_eval,to_string = True)
|
||||
idx = resp.find('\t')
|
||||
return int(resp[idx:],16)
|
||||
|
||||
def is_in_bounds(self,val):
|
||||
lower_bound = int("08004000",16)
|
||||
upper_bound = int("080ae0c0",16);
|
||||
#print(lower_bound," ",val," ",upper_bound)
|
||||
if val>lower_bound and val<upper_bound:
|
||||
return True;
|
||||
else:
|
||||
return False;
|
||||
def get_tcb_from_address(self,addr):
|
||||
addr_value = gdb.Value(addr)
|
||||
tcb_ptr = addr_value.cast(gdb.lookup_type('struct tcb_s').pointer())
|
||||
return tcb_ptr.dereference()
|
||||
|
||||
def print_instruction_at(self,addr,stack_percentage):
|
||||
gdb.write(str(round(stack_percentage,2))+":")
|
||||
str_to_eval = "info line *"+hex(addr)
|
||||
#gdb.execute(str_to_eval)
|
||||
res = gdb.execute(str_to_eval,to_string = True)
|
||||
# get information from results string:
|
||||
words = res.split()
|
||||
valid = False
|
||||
if words[0] == 'No':
|
||||
#no line info...
|
||||
pass
|
||||
else:
|
||||
valid = True
|
||||
line = int(words[1])
|
||||
idx = words[3].rfind("/"); #find first backslash
|
||||
if idx>0:
|
||||
name = words[3][idx+1:];
|
||||
path = words[3][:idx];
|
||||
else:
|
||||
name = words[3];
|
||||
path = "";
|
||||
block = gdb.block_for_pc(addr)
|
||||
func = block.function
|
||||
if str(func) == "None":
|
||||
func = block.superblock.function
|
||||
|
||||
if valid:
|
||||
print("Line: ",line," in ",path,"/",name,"in ",func)
|
||||
return name,path,line,func
|
||||
|
||||
|
||||
|
||||
|
||||
def invoke(self,args,sth):
|
||||
addr_dec = int(args[2:],16)
|
||||
_tcb = self.get_tcb_from_address(addr_dec)
|
||||
print("found task with PID: ",_tcb["pid"])
|
||||
up_stack = int(_tcb['adj_stack_ptr'])
|
||||
curr_sp = int(_tcb['xcp']['regs'][0]) #curr stack pointer
|
||||
other_sp = int(_tcb['xcp']['regs'][8]) # other stack pointer
|
||||
stacksize = int(_tcb['adj_stack_size']) # other stack pointer
|
||||
|
||||
print("tasks current SP = ",hex(curr_sp),"stack max ptr is at ",hex(up_stack))
|
||||
|
||||
if curr_sp == up_stack:
|
||||
sp = other_sp
|
||||
else:
|
||||
sp = curr_sp;
|
||||
|
||||
while(sp < up_stack):
|
||||
mem = self.readmem(sp)
|
||||
#print(hex(sp)," : ",hex(mem))
|
||||
if self.is_in_bounds(mem):
|
||||
# this is a potential instruction ptr
|
||||
stack_percentage = (up_stack-sp)/stacksize
|
||||
name,path,line,func = self.print_instruction_at(mem,stack_percentage)
|
||||
sp = sp + 4; # jump up one word
|
||||
|
||||
NX_my_bt()
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
2
NuttX
2
NuttX
|
|
@ -1 +1 @@
|
|||
Subproject commit 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211c
|
||||
Subproject commit 9d06b645790e1445f14e3b19c71d40b3088f4e4f
|
||||
|
|
@ -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)
|
||||
|
|
@ -1,14 +1,10 @@
|
|||
#!nsh
|
||||
#
|
||||
# HILStar / X-Plane
|
||||
#
|
||||
# Lorenz Meier <lm@inf.ethz.ch>
|
||||
# HILStar
|
||||
# <lorenz@px4.io>
|
||||
#
|
||||
|
||||
sh /etc/init.d/rc.fw_defaults
|
||||
|
||||
echo "X Plane HIL starting.."
|
||||
|
||||
set HIL yes
|
||||
|
||||
set MIXER FMU_AERT
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -2,8 +2,7 @@
|
|||
#
|
||||
# Steadidrone QU4D
|
||||
#
|
||||
# Thomas Gubler <thomasgubler@gmail.com>
|
||||
# Lorenz Meier <lm@inf.ethz.ch>
|
||||
# Thomas Gubler <thomas@px4.io>
|
||||
#
|
||||
|
||||
sh /etc/init.d/rc.mc_defaults
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
#
|
||||
# HIL Quadcopter +
|
||||
#
|
||||
# Anton Babushkin <anton.babushkin@me.com>
|
||||
# Anton Babushkin <anton@px4.io>
|
||||
#
|
||||
|
||||
sh /etc/init.d/rc.mc_defaults
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
#
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -13,3 +13,5 @@ ekf_att_pos_estimator start
|
|||
#
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
||||
|
||||
bottle_drop start
|
||||
|
|
|
|||
|
|
@ -1,10 +0,0 @@
|
|||
#!nsh
|
||||
#
|
||||
# Test jig startup script
|
||||
#
|
||||
|
||||
echo "[testing] doing production test.."
|
||||
|
||||
tests jig
|
||||
|
||||
echo "[testing] testing done"
|
||||
|
|
@ -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.
|
||||
# IMPORTANT: this also loads param offsets
|
||||
# ALWAYS start this task before the
|
||||
# preflight_check.
|
||||
# Start sensors -> preflight_check
|
||||
#
|
||||
if sensors start
|
||||
then
|
||||
|
|
|
|||
|
|
@ -137,9 +137,7 @@ then
|
|||
#
|
||||
if param compare SYS_AUTOCONFIG 1
|
||||
then
|
||||
# We can't be sure the defaults haven't changed, so
|
||||
# if someone requests a re-configuration, we do it
|
||||
# cleanly from scratch (except autostart / autoconfig)
|
||||
# Wipe out params
|
||||
param reset_nostart
|
||||
set DO_AUTOCONFIG yes
|
||||
else
|
||||
|
|
@ -202,12 +200,10 @@ then
|
|||
|
||||
if px4io checkcrc $IO_FILE
|
||||
then
|
||||
echo "[init] PX4IO CRC OK"
|
||||
echo "PX4IO CRC OK" >> $LOG_FILE
|
||||
|
||||
set IO_PRESENT yes
|
||||
else
|
||||
echo "[init] Trying to update"
|
||||
echo "PX4IO Trying to update" >> $LOG_FILE
|
||||
|
||||
tone_alarm MLL32CP8MB
|
||||
|
|
@ -217,18 +213,15 @@ then
|
|||
usleep 500000
|
||||
if px4io checkcrc $IO_FILE
|
||||
then
|
||||
echo "[init] PX4IO CRC OK, update successful"
|
||||
echo "PX4IO CRC OK after updating" >> $LOG_FILE
|
||||
tone_alarm MLL8CDE
|
||||
|
||||
set IO_PRESENT yes
|
||||
else
|
||||
echo "[init] ERROR: PX4IO update failed"
|
||||
echo "PX4IO update failed" >> $LOG_FILE
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
fi
|
||||
else
|
||||
echo "[init] ERROR: PX4IO update failed"
|
||||
echo "PX4IO update failed" >> $LOG_FILE
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
fi
|
||||
|
|
@ -281,16 +274,12 @@ then
|
|||
fi
|
||||
fi
|
||||
|
||||
#
|
||||
# Start the datamanager (and do not abort boot if it fails)
|
||||
#
|
||||
# waypoint storage
|
||||
if dataman start
|
||||
then
|
||||
fi
|
||||
|
||||
#
|
||||
# Start the Commander (needs to be this early for in-air-restarts)
|
||||
#
|
||||
# Needs to be this early for in-air-restarts
|
||||
commander start
|
||||
|
||||
#
|
||||
|
|
@ -424,9 +413,6 @@ then
|
|||
fi
|
||||
fi
|
||||
|
||||
#
|
||||
# MAVLink
|
||||
#
|
||||
if [ $MAVLINK_FLAGS == default ]
|
||||
then
|
||||
# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
|
||||
|
|
@ -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 ]
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
S: 2 2 -10000 -10000 0 -10000 10000
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
self.__printCSVRow()
|
||||
if not self.__debug_out and self.__time_msg != None and self.__csv_updated:
|
||||
self.__printCSVRow()
|
||||
f.close()
|
||||
|
||||
def __bytesLeft(self):
|
||||
|
|
|
|||
|
|
@ -1,4 +1,6 @@
|
|||
./obj/*
|
||||
mixer_test
|
||||
sf0x_test
|
||||
sbus2_test
|
||||
autodeclination_test
|
||||
st24_test
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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/hott/hott_sensors
|
||||
MODULES += drivers/blinkm
|
||||
#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.
|
||||
#
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
#
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
#
|
||||
|
|
|
|||
|
|
@ -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,$<,$@)
|
||||
|
|
|
|||
|
|
@ -50,11 +50,11 @@ OBJDUMP = $(CROSSDEV)objdump
|
|||
|
||||
# Check if the right version of the toolchain is available
|
||||
#
|
||||
CROSSDEV_VER_SUPPORTED = 4.7
|
||||
CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4
|
||||
CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion)
|
||||
|
||||
ifeq (,$(findstring $(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND)))
|
||||
$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED).x)
|
||||
ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED)))
|
||||
$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED))
|
||||
endif
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
Subproject commit 2423e47b4f9169e77f7194b36fa2118e018c94e2
|
||||
Subproject commit ad5e5a645dec152419264ad32221f7c113ea5c30
|
||||
|
|
@ -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 \
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
#
|
||||
|
|
|
|||
|
|
@ -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 \
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -89,8 +89,8 @@ static void
|
|||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d <UART>]\n\n");
|
||||
warnx("%s\n", reason);
|
||||
warnx("usage: {start|stop|status} [-d <UART>]\n\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
|
@ -110,7 +110,7 @@ int ardrone_interface_main(int argc, char *argv[])
|
|||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("ardrone_interface already running\n");
|
||||
warnx("already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
|
@ -132,9 +132,9 @@ int ardrone_interface_main(int argc, char *argv[])
|
|||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tardrone_interface is running\n");
|
||||
warnx("running");
|
||||
} else {
|
||||
printf("\tardrone_interface not started\n");
|
||||
warnx("not started");
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
|
@ -158,7 +158,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
|
|||
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
|
||||
fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
|
||||
warnx("ERR: TCGETATTR %s: %d", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
|
@ -171,14 +171,14 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
|
|||
|
||||
/* Set baud rate */
|
||||
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
||||
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
|
||||
warnx("ERR: cfsetispeed %s: %d", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
|
||||
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
|
||||
warnx("ERR: tcsetattr: %s", uart_name);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
|
@ -192,9 +192,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
|
|||
|
||||
char *device = "/dev/ttyS1";
|
||||
|
||||
/* welcome user */
|
||||
printf("[ardrone_interface] Control started, taking over motors\n");
|
||||
|
||||
/* File descriptors */
|
||||
int gpios;
|
||||
|
||||
|
|
@ -237,7 +234,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
|
|||
struct termios uart_config_original;
|
||||
|
||||
if (motor_test_mode) {
|
||||
printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
|
||||
warnx("setting 10 %% thrust.\n");
|
||||
}
|
||||
|
||||
/* Led animation */
|
||||
|
|
@ -255,9 +252,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
|
|||
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
|
||||
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
|
||||
printf("[ardrone_interface] Motors initialized - ready.\n");
|
||||
fflush(stdout);
|
||||
|
||||
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
|
||||
ardrone_write = ardrone_open_uart(device, &uart_config_original);
|
||||
|
||||
|
|
@ -265,7 +259,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
|
|||
gpios = ar_multiplexing_init();
|
||||
|
||||
if (ardrone_write < 0) {
|
||||
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
|
||||
warnx("No UART, exiting.");
|
||||
thread_running = false;
|
||||
exit(ERROR);
|
||||
}
|
||||
|
|
@ -273,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
|
|||
/* initialize motors */
|
||||
if (OK != ar_init_motors(ardrone_write, gpios)) {
|
||||
close(ardrone_write);
|
||||
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
|
||||
warnx("motor init fail");
|
||||
thread_running = false;
|
||||
exit(ERROR);
|
||||
}
|
||||
|
|
@ -294,7 +288,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
|
|||
gpios = ar_multiplexing_init();
|
||||
|
||||
if (ardrone_write < 0) {
|
||||
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
|
||||
warnx("write fail");
|
||||
thread_running = false;
|
||||
exit(ERROR);
|
||||
}
|
||||
|
|
@ -302,7 +296,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
|
|||
/* initialize motors */
|
||||
if (OK != ar_init_motors(ardrone_write, gpios)) {
|
||||
close(ardrone_write);
|
||||
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
|
||||
warnx("motor init fail");
|
||||
thread_running = false;
|
||||
exit(ERROR);
|
||||
}
|
||||
|
|
@ -378,11 +372,9 @@ int ardrone_interface_thread_main(int argc, char *argv[])
|
|||
int termios_state;
|
||||
|
||||
if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
|
||||
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
|
||||
warnx("ERR: tcsetattr");
|
||||
}
|
||||
|
||||
printf("[ardrone_interface] Restored original UART config, exiting..\n");
|
||||
|
||||
/* close uarts */
|
||||
close(ardrone_write);
|
||||
ar_multiplexing_deinit(gpios);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
****************************************************************************/
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -6,3 +6,5 @@ SRCS = aerocore_init.c \
|
|||
aerocore_pwm_servo.c \
|
||||
aerocore_spi.c \
|
||||
aerocore_led.c
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \
|
|||
px4fmu_spi.c \
|
||||
px4fmu_usb.c \
|
||||
px4fmu_led.c
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \
|
|||
px4fmu_spi.c \
|
||||
px4fmu_usb.c \
|
||||
px4fmu2_led.c
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
|||
|
|
@ -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
|
||||
****************************************************************************/
|
||||
|
|
|
|||
|
|
@ -4,3 +4,5 @@
|
|||
|
||||
SRCS = px4io_init.c \
|
||||
px4io_pwm_servo.c
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
|||
|
|
@ -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 *******************************************************/
|
||||
|
||||
|
|
|
|||
|
|
@ -4,3 +4,5 @@
|
|||
|
||||
SRCS = px4iov2_init.c \
|
||||
px4iov2_pwm_servo.c
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -130,7 +130,8 @@ public:
|
|||
enum DeviceBusType {
|
||||
DeviceBusType_UNKNOWN = 0,
|
||||
DeviceBusType_I2C = 1,
|
||||
DeviceBusType_SPI = 2
|
||||
DeviceBusType_SPI = 2,
|
||||
DeviceBusType_UAVCAN = 3,
|
||||
};
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
/*
|
||||
*
|
||||
|
|
|
|||
|
|
@ -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
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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_ */
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@ MODULE_COMMAND = gps
|
|||
SRCS = gps.cpp \
|
||||
gps_helper.cpp \
|
||||
mtk.cpp \
|
||||
ashtech.cpp \
|
||||
ubx.cpp
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -1049,11 +1049,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
|||
* LSM/Ga, giving 1.16 and 1.08 */
|
||||
float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
|
||||
|
||||
warnx("starting mag scale calibration");
|
||||
|
||||
/* start the sensor polling at 50 Hz */
|
||||
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
|
||||
warn("failed to set 2Hz poll rate");
|
||||
warn("FAILED: SENSORIOCSPOLLRATE 2Hz");
|
||||
ret = 1;
|
||||
goto out;
|
||||
}
|
||||
|
|
@ -1061,25 +1059,25 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
|||
/* Set to 2.5 Gauss. We ask for 3 to get the right part of
|
||||
* the chained if statement above. */
|
||||
if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
|
||||
warnx("failed to set 2.5 Ga range");
|
||||
warnx("FAILED: MAGIOCSRANGE 3.3 Ga");
|
||||
ret = 1;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
|
||||
warnx("failed to enable sensor calibration mode");
|
||||
warnx("FAILED: MAGIOCEXSTRAP 1");
|
||||
ret = 1;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
|
||||
warn("WARNING: failed to get scale / offsets for mag");
|
||||
warn("FAILED: MAGIOCGSCALE 1");
|
||||
ret = 1;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
|
||||
warn("WARNING: failed to set null scale / offsets for mag");
|
||||
warn("FAILED: MAGIOCSSCALE 1");
|
||||
ret = 1;
|
||||
goto out;
|
||||
}
|
||||
|
|
@ -1094,7 +1092,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
|||
ret = ::poll(&fds, 1, 2000);
|
||||
|
||||
if (ret != 1) {
|
||||
warn("timed out waiting for sensor data");
|
||||
warn("ERROR: TIMEOUT 1");
|
||||
goto out;
|
||||
}
|
||||
|
||||
|
|
@ -1102,7 +1100,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
|||
sz = ::read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report)) {
|
||||
warn("periodic read failed");
|
||||
warn("ERROR: READ 1");
|
||||
ret = -EIO;
|
||||
goto out;
|
||||
}
|
||||
|
|
@ -1118,7 +1116,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
|||
ret = ::poll(&fds, 1, 2000);
|
||||
|
||||
if (ret != 1) {
|
||||
warn("timed out waiting for sensor data");
|
||||
warn("ERROR: TIMEOUT 2");
|
||||
goto out;
|
||||
}
|
||||
|
||||
|
|
@ -1126,7 +1124,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
|||
sz = ::read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report)) {
|
||||
warn("periodic read failed");
|
||||
warn("ERROR: READ 2");
|
||||
ret = -EIO;
|
||||
goto out;
|
||||
}
|
||||
|
|
@ -1142,33 +1140,19 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
|||
sum_excited[1] += cal[1];
|
||||
sum_excited[2] += cal[2];
|
||||
}
|
||||
|
||||
//warnx("periodic read %u", i);
|
||||
//warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
|
||||
//warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
|
||||
}
|
||||
|
||||
if (good_count < 5) {
|
||||
warn("failed calibration");
|
||||
ret = -EIO;
|
||||
goto out;
|
||||
}
|
||||
|
||||
#if 0
|
||||
warnx("measurement avg: %.6f %.6f %.6f",
|
||||
(double)sum_excited[0]/good_count,
|
||||
(double)sum_excited[1]/good_count,
|
||||
(double)sum_excited[2]/good_count);
|
||||
#endif
|
||||
|
||||
float scaling[3];
|
||||
|
||||
scaling[0] = sum_excited[0] / good_count;
|
||||
scaling[1] = sum_excited[1] / good_count;
|
||||
scaling[2] = sum_excited[2] / good_count;
|
||||
|
||||
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
|
||||
|
||||
/* set scaling in device */
|
||||
mscale_previous.x_scale = scaling[0];
|
||||
mscale_previous.y_scale = scaling[1];
|
||||
|
|
@ -1179,29 +1163,26 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
|||
out:
|
||||
|
||||
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
|
||||
warn("failed to set new scale / offsets for mag");
|
||||
warn("FAILED: MAGIOCSSCALE 2");
|
||||
}
|
||||
|
||||
/* set back to normal mode */
|
||||
/* Set to 1.1 Gauss */
|
||||
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
|
||||
warnx("failed to set 1.1 Ga range");
|
||||
warnx("FAILED: MAGIOCSRANGE 1.1 Ga");
|
||||
}
|
||||
|
||||
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
|
||||
warnx("failed to disable sensor calibration mode");
|
||||
warnx("FAILED: MAGIOCEXSTRAP 0");
|
||||
}
|
||||
|
||||
if (ret == OK) {
|
||||
if (!check_scale()) {
|
||||
warnx("mag scale calibration successfully finished.");
|
||||
} else {
|
||||
warnx("mag scale calibration finished with invalid results.");
|
||||
if (check_scale()) {
|
||||
/* failed */
|
||||
warnx("FAILED: SCALE");
|
||||
ret = ERROR;
|
||||
}
|
||||
|
||||
} else {
|
||||
warnx("mag scale calibration failed.");
|
||||
}
|
||||
|
||||
return ret;
|
||||
|
|
|
|||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue